Adding Video Capture in ROS 2

Video with RVIZ2

In this tutorial, you will learn how to configure your own RVIZ2 session to see only the video data that you require.

To visualize the video stream published by the ZED node, you can use two different plugins:

  • Camera: Displays an image from a camera, with the visualized world rendered behind it
  • Image: Displays an image from a topic of type sensor_msgs/Image

Camera

The Camera plugin allows you to visualize an image from a topic of type sensor_msgs/Image. It acts as if the source of the image is placed on its virtual frame and renders all virtual objects in front of it.

Key parameters:

  • Topic: Selects the image topic to visualize from the list of available images in the combo box
  • Depth: The depth of the incoming message queue
  • History policy: Set the QoS history policy. Keep Last is suggested for performances and compatibility
  • Reliability Policy: Set the QoS reliability policy. Best Effort is suggested for performances and compatibility
  • Durability Policy: Set the QoS durability policy. Volatile is suggested for compatibility

By expanding Visibility, you can select/deselect what will be visible in the camera view. Only active plugins can be selected.

Image

The Image plugin allows you to visualize an image from a topic of type sensor_msgs/Image.

Key parameters:

  • Topic: Selects the image topic to visualize from the list of available images in the combo box
  • Depth: The depth of the incoming message queue
  • History policy: Set the QoS history policy. Keep Last is suggested for performances and compatibility
  • Reliability Policy: Set the QoS reliability policy. Best Effort is suggested for performances and compatibility
  • Durability Policy: Set the QoS durability policy. Volatile is suggested for compatibility

Video subscribing in C++

In this tutorial, you will learn how to write a simple C++ node that subscribes to messages of type sensor_msgs/Image. This lets you retrieve the Left and Right rectified images published by the ZED node.

Introduction

Open a new console and use this command to connect the camera to the ROS2 network:

ZED:

$ ros2 launch zed_wrapper zed.launch.py

ZED-M:

$ ros2 launch zed_wrapper zedm.launch.py

ZED2:

$ ros2 launch zed_wrapper zed2.launch.py

The ZED node will start to publish image data in the network only if there is another node that subscribes to the relative topic.

Running the tutorial

If you properly followed the ROS2 Examples Installation Guide, the executable of this tutorial has been compiled and you can run the subscriber node using this command:

$ ros2 run stereolabs_zed_tutorial_video stereolabs_zed_tutorial_video

The tutorial node subscribes generic left_image and right_image topics, so a remapping is required to connect to the correct topics published by the ZED node:

ZED:

$ ros2 run zed_tutorial_video zed_tutorial_video --ros-args -r left_image:=/zed/zed_node/left/image_rect_color -r right_image:=/zed/zed_node/right/image_rect_color

ZED-M:

$ ros2 run zed_tutorial_video zed_tutorial_video --ros-args -r left_image:=/zedm/zed_node/left/image_rect_color -r right_image:=/zedm/zed_node/right/image_rect_color

ZED2:

$ ros2 run zed_tutorial_video zed_tutorial_video --ros-args -r left_image:=/zed2/zed_node/left/image_rect_color -r right_image:=/zed2/zed_node/right/image_rect_color

If the ZED node is running, and a camera is connected or you have loaded an SVO file, you will receive the following stream of messages confirming that you have correctly subscribed to the ZED image topics:

[INFO] [zed_video_tutorial]: Left  Rectified image received from ZED	Size: 1280x720 - Timestamp: 1602576933.791896880 sec 
[INFO] [zed_video_tutorial]: Right Rectified image received from ZED	Size: 1280x720 - Timestamp: 1602576933.891931106 sec 
[INFO] [zed_video_tutorial]: Left  Rectified image received from ZED	Size: 1280x720 - Timestamp: 1602576934.91928857 sec 
[INFO] [zed_video_tutorial]: Right Rectified image received from ZED	Size: 1280x720 - Timestamp: 1602576934.91928857 sec 
[INFO] [zed_video_tutorial]: Left  Rectified image received from ZED	Size: 1280x720 - Timestamp: 1602576934.191911460 sec 
[INFO] [zed_video_tutorial]: Right Rectified image received from ZED	Size: 1280x720 - Timestamp: 1602576934.191911460 sec

[...]

The code

The source code of the subscriber node zed_video_sub_tutorial.cpp:

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>
#include <sensor_msgs/msg/image.hpp>

rclcpp::Node::SharedPtr g_node = nullptr;

/**
 * Subscriber callbacks. The argument of the callback is a constant pointer to the received message
 */


void imageRightRectifiedCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
    RCLCPP_INFO(g_node->get_logger(),
                "Right Rectified image received from ZED\tSize: %dx%d - Timestamp: %u.%u sec ",
                msg->width, msg->height,
                msg->header.stamp.sec,msg->header.stamp.nanosec);
}

void imageLeftRectifiedCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
    RCLCPP_INFO(g_node->get_logger(),
                "Left  Rectified image received from ZED\tSize: %dx%d - Timestamp: %u.%u sec ",
                msg->width, msg->height,
                msg->header.stamp.sec,msg->header.stamp.nanosec);
}

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    // Create the node
    g_node = rclcpp::Node::make_shared("zed_video_tutorial");

    /* Note: it is very important to use a QOS profile for the subscriber that is compatible
     * with the QOS profile of the publisher.
     * The ZED component node uses a default QoS profile with reliability set as "RELIABLE"
     * and durability set as "VOLATILE".
     * To be able to receive the subscribed topic the subscriber must use compatible
     * parameters.
     */

    // https://github.com/ros2/ros2/wiki/About-Quality-of-Service-Settings

    rclcpp::QoS video_qos(10);
    video_qos.keep_last(10);
    video_qos.best_effort();
    video_qos.durability_volatile();

    // Create right image subscriber
    auto right_sub = g_node->create_subscription<sensor_msgs::msg::Image>(
                "right_image", video_qos, imageRightRectifiedCallback );

    // Create left image subscriber
    auto left_sub = g_node->create_subscription<sensor_msgs::msg::Image>
            ("left_image", video_qos, imageLeftRectifiedCallback );

    // Let the node run
    rclcpp::spin(g_node);

    // Shutdown when the node is stopped using Ctrl+C
    rclcpp::shutdown();

    return 0;
}

The code explained

The following is a brief explanation about the above source code:

void imageRightRectifiedCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
    RCLCPP_INFO(g_node->get_logger(),
                "Right Rectified image received from ZED\tSize: %dx%d - Timestamp: %u.%u sec ",
                msg->width, msg->height,
                msg->header.stamp.sec,msg->header.stamp.nanosec);
}

void imageLeftRectifiedCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
    RCLCPP_INFO(g_node->get_logger(),
                "Left  Rectified image received from ZED\tSize: %dx%d - Timestamp: %u.%u sec ",
                msg->width, msg->height,
                msg->header.stamp.sec,msg->header.stamp.nanosec);
}

These callbacks are executed when the subscriber node receives a message of type sensor_msgs/Image that matches the subscribed topic. The parameter of the callback is a std::shared_ptr to the received message. This means you don’t have to worry about memory management. The callback code is very simple and demonstrates how to access the fields in a message; in this case, the height and width of the image and the topic timestamp.

The main function in divided in two main parts:

Node declaration:

    rclcpp::init(argc, argv);

    // Create the node
    g_node = rclcpp::Node::make_shared("zed_video_tutorial");

First, the ROS2 environment is initialized with the rclcpp::init command. Then, the zed_video_tutorial node is created and a shared pointer g_node to it is initialized.

The most important lesson of the above code is how the subscribers are defined:

    rclcpp::QoS video_qos(10);
    video_qos.keep_last(10);
    video_qos.best_effort();
    video_qos.durability_volatile();

    // Create right image subscriber
    auto right_sub = g_node->create_subscription<sensor_msgs::msg::Image>(
                "right_image", video_qos, imageRightRectifiedCallback );

    // Create left image subscriber
    auto left_sub = g_node->create_subscription<sensor_msgs::msg::Image>(
                "left_image", video_qos, imageLeftRectifiedCallback );

The two auto variables right_sub and left_sub are two rclcpp::Subscription objects.

A rclcpp::Subscription is a ROS object that listens to the network and waits for its own topic message to be available. When a message is received, it executes the callback assigned to it.

We declared two subscribers: one for the left rectified image and one for the right rectified image.

  • The subscriber to the right_image topic calls the imageRightRectCallback function when it receives a message of type sensor_msgs/Image that matches that topic
  • The subscriber to the left_image topic calls the imageLeftRectCallback function when it receives a message of type sensor_msgs/Image that matches that topic

It is important that the two subscriptions use a QOS profile compatible with the QOS profile of the publisher of the topics.

In this case, the QOS profile is configured to keep the last received 10 messages with “best effort” reliability and “volatile” durability. This configuration is highly compatible to many possible publisher configurations.

For more information about QoS compatibility, refer to the ZED node guide

Note: The code of this tutorial instantiates a rclcpp::Node without subclassing it. This was the typical usage model in the original ROS, but unfortunately this style of coding is not compatible with composing multiple nodes into a single process. Thus, it is no longer the recommended style for ROS2. Please refer to the “Depth sensing” tutorial for C++ tutorials written with a coding style more targeted to ROS 2.

Conclusion

The full source code of this tutorial is available on GitHub in the zed_video_tutorial sub-package. Along with the node source code are the package.xml and CMakeLists.txt files that complete the tutorial package.