def __init__(self): self._rs = RobotEnable() self._cvbr = CvBridge() self._sm = RobotStateMachine(self) self.ik = IKHelper() self.ik.set_right(0.5, 0.0, 0.0, wait=True) self.left_img = None self.right_img = None self._left_camera_sub = rospy.Subscriber( '/cameras/left_hand_camera/image_rect_avg', Image, self.on_left_imagemsg_received) self._right_camera_sub = rospy.Subscriber( '/cameras/right_hand_camera/image_rect_avg', Image, self.on_right_imagemsg_received) self._display_pub = rospy.Publisher('/robot/xdisplay', Image, tcp_nodelay=True, latch=True) self.range = None self._range_sub = rospy.Subscriber( '/robot/range/right_hand_range/state', Range, self.on_rangemsg_received) self.itb = None self._itb_sub = rospy.Subscriber('/robot/itb/right_itb/state', ITBState, self.on_itbmsg_received) self.gripper = Gripper('right') self.gripper.calibrate() self.gripper.close(block=True)
def __init__(self, camera_averaging=False): self._cvbr = CvBridge() self.rs = RobotEnable() self.ik = IKHelper() camera_topic = '/cameras/{0}_camera/image_rect' if camera_averaging: camera_topic += '_avg' self.left_img = None self._left_camera_sub = rospy.Subscriber( camera_topic.format('left_hand'), Image, self._on_left_imagemsg_received) self.right_img = None self._right_camera_sub = rospy.Subscriber( camera_topic.format('right_hand'), Image, self._on_right_imagemsg_received) self.head_img = None self._head_camera_sub = rospy.Subscriber( camera_topic.format('head'), Image, self._on_head_imagemsg_received) self.left_itb = None self._left_itb_sub = rospy.Subscriber('/robot/itb/left_itb/state', ITBState, self._on_left_itbmsg_received) self.right_itb = None self._right_itb_sub = rospy.Subscriber('/robot/itb/right_itb/state', ITBState, self._on_right_itbmsg_received) self.left_gripper = Gripper('left') self.right_gripper = Gripper('right') self.head = Head() self._display_pub = rospy.Publisher('/robot/xdisplay', Image, tcp_nodelay=True, latch=True)