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()