Sending more than two images
The teleop interface supports two video streams by default. In order to send more than two images, you can stitch images together prior to streaming them.
The following Python scripts can be used to stitch videos at the source:
Python stitching
import cv2
import numpy as np
def stitch_jpgs(jpg_a, jpg_b):
image_a = cv2.imdecode(np.frombuffer(jpg_a, dtype=np.uint8), cv2.IMREAD_COLOR)
image_b = cv2.imdecode(np.frombuffer(jpg_b, dtype=np.uint8), cv2.IMREAD_COLOR)
image_stitched = np.concatenate([image_a, image_b])
return cv2.imencode(".jpg", img)[1].tostring()
Full ROS stitching node
import rospy
from sensor_msgs.msg import CompressedImage
import cv2
import numpy as np
TOPIC_A = "/path/fisheye_camera_front/image_raw/compressed"
TOPIC_B = "/path/fisheye_camera_back/image_raw/compressed"
STITCH_TOPIC = "/path/fisheye_camera/stitched/compressed"
def ros_jpg_to_cv2(msg):
return cv2.imdecode(np.frombuffer(msg.data, dtype=np.uint8), cv2.IMREAD_COLOR)
def cv2_to_ros_jpg(img):
cv_encode = cv2.imencode(".jpg", img)[1]
compressed_image = np.array(cv_encode).tostring()
compressed_image_msg = CompressedImage()
compressed_image_msg.format = "jpg"
compressed_image_msg.data = compressed_image
return compressed_image_msg
class Stitcher:
def __init__(self):
self.topic_a_img = None
self.topic_b_img = None
self.pub = rospy.Publisher(STITCH_TOPIC, CompressedImage, queue_size=10)
rospy.Subscriber(TOPIC_A, CompressedImage, self.topic_a_callback)
rospy.Subscriber(TOPIC_B, CompressedImage, self.topic_b_callback)
def topic_b_callback(self, msg):
self.topic_b_img = ros_jpg_to_cv2(msg)
# When topic A is received, publish the full image
def topic_a_callback(self, msg):
self.topic_a_img = ros_jpg_to_cv2(msg)
if self.topic_b_img is not None:
stitched = np.concatenate([self.topic_a_img, self.topic_b_img])
self.pub.publish(cv2_to_ros_jpg(stitched))
if __name__ == "__main__":
rospy.init_node("stitcher")
Stitcher()
rospy.spin()
Updated about 1 year ago