Video

Video with RVIZ

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.

Keyparameters:

  • Image topic: Selects the image topic to visualize from the list of available images in the combo box
  • Unreliable: set the QOS profile to BEST_EFFORT if checked
  • Queue size: The size of the message queue. Use a lower value to reduce latency

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:

  • Image topic: Selects the image topic to visualize from the list of available images in the combo box
  • Unreliable: set the QOS profile to BEST_EFFORT if checked
  • Queue size: The size of the message queue. Use a lower value to reduce latency

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

Use this command to connect the ZED camera to the ROS2 network:

$ ros2 launch stereolabs_zed zed.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 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 zed_video_tutorial

If the ZED node is running, and a ZED or a ZED-M 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_sub]: Right Rectified image received from ZED - Size: 1280x720
[INFO] [zed_video_sub]: Left Rectified image received from ZED - Size: 1280x720
[INFO] [zed_video_sub]: Right Rectified image received from ZED - Size: 1280x720
[INFO] [zed_video_sub]: Left Rectified image received from ZED - Size: 1280x720
[INFO] [zed_video_sub]: Right Rectified image received from ZED - Size: 1280x720
[INFO] [zed_video_sub]: Left Rectified image received from ZED - Size: 1280x720

[...]

The code

The source code of the subscriber node zed_video_sub_tutorial.cpp:

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs//msg/image.hpp"

// Global Node pointer
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 - Size: %dx%d", 
    msg->width, msg->height);
}

void imageLeftRectifiedCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
    RCLCPP_INFO(g_node->get_logger(), 
    "Left Rectified image received from ZED - Size: %dx%d", 
    msg->width, msg->height);
}

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

    // Node initialization
    g_node = rclcpp::Node::make_shared("zed_video_sub");

    // https://index.ros.org/doc/ros2/About-Quality-of-Service-Settings
    rmw_qos_profile_t camera_qos_profile = rmw_qos_profile_sensor_data;

    /* 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 node uses a "rmw_qos_profile_sensor_data" profile for depth data,
     * so reliability is "BEST_EFFORT" and durability is "VOLATILE".
     * To be able to receive the subscribed topic the subscriber must use the
     * same parameters, so setting the QOS to "rmw_qos_profile_sensor_data" as the publisher
     * is the better solution.
     */

    auto sub_right = g_node->create_subscription<sensor_msgs::msg::Image>
                     ("/zed/zed_node/right/image_rect_color", imageRightRectifiedCallback,
                      rmw_qos_profile_sensor_data);

    auto sub_left = g_node->create_subscription<sensor_msgs::msg::Image>
                    ("/zed/zed_node/left/image_rect_color", imageLeftRectifiedCallback,
                     rmw_qos_profile_sensor_data);

    rclcpp::spin(g_node);
    
    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 - Size: %dx%d", 
    msg->width, msg->height);
}

void imageLeftRectifiedCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
    RCLCPP_INFO(g_node->get_logger(), 
    "Left Rectified image received from ZED - Size: %dx%d", 
    msg->width, msg->height);
}

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.

The main function in divided in two main parts:

Node declaration:

    rclcpp::init(argc, argv);

    // Node initialization
    g_node = rclcpp::Node::make_shared("zed_video_sub");

First, the ROS2 environment is initialized with the rclcpp::init command. Then, the zed_video_sub 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:

    mw_qos_profile_t camera_qos_profile = rmw_qos_profile_sensor_data;

    // Subscribers
    auto sub_right = g_node->create_subscription<sensor_msgs::msg::Image>
                     ("/zed/zed_node/right/image_rect_color", imageRightRectifiedCallback,
                      rmw_qos_profile_sensor_data);

    auto sub_left = g_node->create_subscription<sensor_msgs::msg::Image>
                    ("/zed/zed_node/left/image_rect_color", imageLeftRectifiedCallback,
                     rmw_qos_profile_sensor_data);

The two auto variables sub_right and sub_left 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 /zed/zed_node/right/image_rect_color topic calls the imageRightRectCallback function when it receives a message of type sensor_msgs/Image that matches that topic
  • The subscriber to the /zed/zed_node/left/image_rect_color 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 used is a predefined profile for sensor data and it is the same one used by the ZED node to publish the image data.

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” or to the “ZED Lifecycle” tutorials 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.