def makeROS(self, data):
# Given a list of the form [{message type (str)}, {message data (dict)}] received via TCP,
# parse and create a ROS message.
print "Data received (" + str(sys.getsizeof(data[1])) + "): " + data[0]
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = RVIZ_FRAME
if data[0] == '/hardware_joint_states':
pass
elif data[0] == '/ihmc_ros/valkyrie/output/joint_states':
msg = JointState()
msg.header = header
msg.name = data[1]['name']
msg.position = data[1]['position']
self.joints_publisher.publish(msg)
elif data[0] == '/tf':
msg = TFMessage()
msg.transforms = data[1]['transforms']
self.tf_publisher.publish(msg)
elif data[0] == '/multisense/camera/points2':
self.points2_publisher.publish(pc2.create_cloud_xyz32(header,data[1]))
elif data[0] == '/multisense/camera/left/image_rect_color':
msg = Image()
msg.header = header
msg.height = data[1]['height']
msg.width = data[1]['width']
msg.encoding = data[1]['encoding']
msg.is_bigendian = data[1]['is_bigendian']
msg.step = data[1]['step']
msg.data = data[1]['data']
self.image_publisher.publish(msg)
bridge = CvBridge()
left_image = cv2.flip(cv2.flip( bridge.imgmsg_to_cv2(msg, "bgr8") ,0),1)
cv2.imshow("left camera",left_image)
cv2.waitKey(3)
else:
print ("WARNING! Unrecognized data packet received. Ignoring data")
