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)
