class Baxter:
def __init__(self, usegripper = True):
rospy.init_node("Baxter")
threading.Thread(target=self.__jointtraj_server).start()
self.trajrgt = Trajectory("right")
self.trajlft = Trajectory("left")
if usegripper:
self.__right_hand = Gripper("right")
self.__left_hand = Gripper("left")
self.__right_hand.calibrate()
self.__left_hand.calibrate()
self.__right_limb = Limb("right")
self.__left_limb = Limb("left")
self.baxter = RobotEnable()
self.__enabled = self.baxter.state().enabled
self.enable_baxter()
self.img = None
for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']:
try:
cam = CameraController(camname)
# cam.resolution = (1280, 800)
cam.resolution = (320, 200)
except:
pass
print("baxter opened")
@property
def enabled(self):
return self.__enabled
def __jointtraj_server(self):
limb = "both"
rate = 100.0
mode = "position"
if mode == 'velocity':
dyn_cfg_srv = Server(VelocityJointTrajectoryActionServerConfig,
lambda config, level: config)
elif mode == 'position':
dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
else:
dyn_cfg_srv = Server(PositionFFJointTrajectoryActionServerConfig,
lambda config, level: config)
jtas = []
if limb == 'both':
jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv,
rate, mode))
jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv,
rate, mode))
else:
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
def cleanup():
for j in jtas:
j.clean_shutdown()
rospy.on_shutdown(cleanup)
print("Running. Ctrl-c to quit")
rospy.spin()
def enable_baxter(self):
print("Opening the baxter")
if not self.__enabled: self.baxter.enable()
self.__enabled = self.baxter.state().enabled
def disable_baxter(self):
if self.__enabled: self.baxter.disable()
self.__enabled = self.baxter.state().enabled
def opengripper(self, armname="rgt"):
if not self.__enabled: return
arm = self.__right_hand if armname == "rgt" else self.__left_hand
if self._is_grippable(arm):
arm.open()
else:
rospy.logwarn(armname+ " have not been calibrated")
def closegripper(self, armname="rgt"):
if not self.__enabled: return
arm = self.__right_hand if armname == "rgt" else self.__left_hand
if self._is_grippable(arm):
arm.close()
else:
rospy.logwarn(armname + " have not been calibrated")
def commandgripper(self,pos , armname="rgt"):
if not self.__enabled: return
arm = self.__right_hand if armname == "rgt" else self.__left_hand
if self._is_grippable(arm):
arm.command_position(position = pos)
else:
rospy.logwarn(armname + " have not been calibrated")
def _is_grippable(self,gripper):
return (gripper.calibrated() and gripper.ready())
def currentposgripper(self,armname ="rgt"):
arm = self.__right_hand if armname == "rgt" else self.__left_hand
return arm.position()
def getjnts(self,armname="rgt"):
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
angles = limb.joint_angles()
return angles
def movejnts(self,jnts_dict, speed =.6 , armname="rgt"):
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
traj = self.trajrgt if armname is "rgt" else self.trajlft
# limb.set_joint_position_speed(speed)
currentjnts = [limb.joint_angle(joint) for joint in limb.joint_names()]
traj.add_point(currentjnts, 0)
if isinstance(jnts_dict, dict):
path_angle = [jnts_dict[joint_name] for joint_name in limb.joint_names()]
else:
path_angle = jnts_dict
# print(path_angle)
traj.add_point(path_angle, speed)
# limb.move_to_joint_positions(jnts_dict,threshold= baxter_interface.settings.JOINT_ANGLE_TOLERANCE)
limb.set_joint_position_speed(.3)
def movejnts_cont(self, jnts_dict_list, speed=.1, armname="rgt"):
traj = self.trajrgt if armname is "rgt" else self.trajlft
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
# limb.set_joint_position_speed(.6)
currentjnts = [limb.joint_angle(joint) for joint in limb.joint_names()]
traj.add_point(currentjnts, 0)
rospy.on_shutdown(traj.stop)
t = speed
for jnts_dict in jnts_dict_list:
# limb.move_to_joint_positions(jnts_dict, threshold= baxter_interface.settings.JOINT_ANGLE_TOLERANCE)
if isinstance(jnts_dict,dict):
path_angle = [jnts_dict[joint_name] for joint_name in limb.joint_names()]
else:
path_angle = jnts_dict
# print(path_angle)
traj.add_point(path_angle,t)
t += speed
traj.start()
traj.wait(-1)
limb.set_joint_position_speed(.3)
traj.clear()
def getforce(self,armname="rgt"):
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
ft = limb.endpoint_effort()
force = ft['force']
torque = ft['torque']
print ft
return list(force+torque)
def setscreenimage(self,path):
# it seems not working
"""
Send the image located at the specified path to the head
display on Baxter.
@param path: path to the image file to load and send
"""
# rospy.init_node('rsdk_xdisplay_image', anonymous=True)
print("Changing the photo")
img = cv.imread(path)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/robot_s/xdisplay', Image, latch=True, queue_size=1)
pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(1)
def getimage(self,cameraname="left_hand_camera"):
# ['head_camera', 'left_hand_camera', 'right_hand_camera']
# if cameraname != self.camera_on:
# cam = CameraController(self.camera_on)
# cam.close()
# cam = CameraController(cameraname)
# cam.resolution = (1280, 800)
# cam.open()
# self.camera_on = cameraname
print("Getting imag...")
"rosrun image_view image_view image:=/cameras/left_hand_camera/image"
"rosrun image_view image_view image:=/cameras/head_camera/image"
# Instantiate CvBridge
bridge = cv_bridge.CvBridge()
def image_callback(msg):
print("Received an image!")
try:
# Convert your ROS Image message to OpenCV2
cv_img = bridge.imgmsg_to_cv2(msg, "bgr8")
except cv_bridge.CvBridgeError, e:
print(e)
else:
