class BlockStackerNode(object):
RATE = 250
############################################################################
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 start(self):
self._rs.enable()
self._sm.start()
rate = rospy.Rate(BlockStackerNode.RATE)
while not rospy.is_shutdown():
self._sm.run_step()
rate.sleep()
############################################################################
def on_left_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.left_img = img.copy()
self._sm.on_left_image_received(img)
def on_right_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.right_img = img.copy()
self._sm.on_right_image_received(img)
def on_rangemsg_received(self, range_msg):
self.range = range_msg.range
def on_itbmsg_received(self, itb_msg):
self.itb = itb_msg
def display_image(self, img):
img = cv2.resize(img, (1024, 600))
print img.shape
if img.shape[2] == 1:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
self._display_pub.publish(img_msg)
class BlockStackerNode(object):
RATE = 250
############################################################################
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 start(self):
self._rs.enable()
self._sm.start()
rate = rospy.Rate(BlockStackerNode.RATE)
while not rospy.is_shutdown():
self._sm.run_step()
rate.sleep()
############################################################################
def on_left_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.left_img = img.copy()
self._sm.on_left_image_received(img)
def on_right_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.right_img = img.copy()
self._sm.on_right_image_received(img)
def on_rangemsg_received(self, range_msg):
self.range = range_msg.range
def on_itbmsg_received(self, itb_msg):
self.itb = itb_msg
def display_image(self, img):
img = cv2.resize(img, (1024, 600))
print img.shape
if img.shape[2] == 1:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
self._display_pub.publish(img_msg)