def _publish_image(self): """ Publish latest camera image as Image with CameraInfo. """ # only publish if we have a subscriber if self._image_pub.get_num_connections() == 0: return # get latest image from vectors's camera camera_image = self._vector.camera.latest_image if camera_image is not None: # convert image to gray scale img = camera_image.convert('RGB') # 640,360 image size? # img = camera_image ros_img = Image() ros_img.encoding = 'rgb8' ros_img.width = img.size[0] ros_img.height = img.size[1] ros_img.step = ros_img.width ros_img.data = img.tobytes() ros_img.header.frame_id = 'vector_camera' # vector_time = camera_image.image_recv_time # ros_img.header.stamp = rospy.Time.from_sec(vector_time) ros_img.header.stamp = rospy.Time.now() # publish images and camera info self._image_pub.publish(ros_img)
def numpy_to_imgmsg(image, stamp=None): from sensor_msgs.msg import Image rosimage = Image() rosimage.height = image.shape[0] rosimage.width = image.shape[1] if image.dtype == numpy.uint8: rosimage.encoding = '8UC%d' % image.shape[2] rosimage.step = image.shape[2] * rosimage.width rosimage.data = image.ravel().tolist() else: rosimage.encoding = '32FC%d' % image.shape[2] rosimage.step = image.shape[2] * rosimage.width * 4 rosimage.data = numpy.array(image.flat, dtype=numpy.float32).tostring() if stamp is not None: rosimage.header.stamp = stamp return rosimage
def pil_to_imgmsg(image, encodingmap={'L': 'mono8', 'RGB': 'rgb8', 'RGBA': 'rgba8', 'YCbCr': 'yuv422'}, PILmode_channels={'L': 1, 'RGB': 3, 'RGBA': 4, 'YCbCr': 3}): # import roslib # @UnresolvedImport @UnusedImport # import rospy # @UnresolvedImport @UnusedImport from sensor_msgs.msg import Image # from sensor_msgs.msg import CompressedImage # @UnusedImport @UnresolvedImport rosimage = Image() # adam print 'Channels image.mode: ',PILmode_channels[image.mode] rosimage.encoding = encodingmap[image.mode] (rosimage.width, rosimage.height) = image.size rosimage.step = PILmode_channels[image.mode] * rosimage.width rosimage.data = image.tostring() return rosimage
def callback(self, data): #points.append(171,224) data.data points_msg = Image() bridge = CvBridge() IMAGE_HEIGHT = 171 IMAGE_WIDTH = 224 points_msg.header.stamp = rospy.Time.now() points_msg.header.frame_id = "camera_depth_optical_frame" points_msg.encoding = "32FC1" points_msg.height = IMAGE_HEIGHT points_msg.width = IMAGE_WIDTH points_msg.data = bridge.cv2_to_imgmsg(points_img, '32FC1').data points_msg.is_bigendian = 0 points_msg.step = points_msg.width * 4 cvImage = bridge.cv2_to_imgmsg(points_img, encoding='passthrough') cvImPub = rospy.Publisher('/cvImage_array', Image, queue_size=1) cvImPub.publish(points_msg)