Source code for morse.middleware.ros.video_camera

import logging; logger = logging.getLogger("morse." + __name__)
import rospy
from sensor_msgs.msg import Image, CameraInfo
from morse.middleware.ros import ROSPublisher, ROSPublisherTF

[docs]class CameraPublisher(ROSPublisherTF): """ Publish the image from the Camera perspective. And send the intrinsic matrix information in a separate topic of type `sensor_msgs/CameraInfo <http://ros.org/wiki/rviz/DisplayTypes/Camera>`_. """ ros_class = Image encoding = 'tbd' pub_tf = True
[docs] def initialize(self): if not 'topic_suffix' in self.kwargs: self.kwargs['topic_suffix'] = '/image' self.pub_tf = self.kwargs.get('pub_tf', True) if self.pub_tf: ROSPublisherTF.initialize(self) else: ROSPublisher.initialize(self) # Generate a publisher for the CameraInfo self.topic_camera_info = rospy.Publisher(self.topic_name+'/camera_info', CameraInfo)
[docs] def finalize(self): if self.pub_tf: ROSPublisherTF.finalize(self) else: ROSPublisher.finalize(self) # Unregister the CameraInfo topic self.topic_camera_info.unregister()
[docs] def default(self, ci='unused'): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = self.encoding image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.D = [0] camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]] camera_info.R = R camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0] if self.pub_tf: self.publish_with_robot_transform(image) else: self.publish(image) self.topic_camera_info.publish(camera_info)
[docs]class VideoCameraPublisher(CameraPublisher): encoding = 'rgba8'
[docs]class DepthCameraPublisher(CameraPublisher): encoding = '32FC1'