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)
