ROS2 composition and ZED ROS2 Wrapper

ROS2 expanded the concept of nodelet from ROS1 replacing nodelets with components and introducing the new concept of “Composition”.

Each node can be written as a “Component” that is a shared library loaded at runtime.

In this way it is possible to

run multiple nodes in separate processes with the benefits of process/fault isolation as well as easier debugging of individual nodes
run multiple nodes in a single process with the lower overhead and optionally more efficient communication (see Intra Process Communication).

The ZED ROS2 wrapper creates the main component, stereolabs::ZedCamera, in the zed_components package. The previous sections of this documentation used the package zed_wrapper to start the ZED node in a single process, named zed_wrapper using the concept of “manual composition” in C++.

The component stereolabs::ZedCamera is loaded by a rclcpp::executors::SingleThreadedExecutor container and executed “alone”::

#include <rclcpp/rclcpp.hpp>
#include "zed_components/zed_camera_component.hpp"

int main(int argc, char * argv[])
{
    // Force flush of the stdout buffer.
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);

    // Initialize any global resources needed by the middleware and the client library.
    // This will also parse command line arguments one day (as of Beta 1 they are not used).
    // You must call this before using any other part of the ROS system.
    // This should be called once per process.
    rclcpp::init(argc, argv);

    // Create an executor that will be responsible for execution of callbacks for a set of nodes.
    // With this version, all callbacks will be called from within this thread (the main one).
    rclcpp::executors::SingleThreadedExecutor exec;
    rclcpp::NodeOptions options;

    // Add zed_camera node
    auto zed_node = std::make_shared<stereolabs::ZedCamera>(options);
    exec.add_node(zed_node);

    // spin will block until work comes in, execute work as it becomes available, and keep blocking.
    // It will only be interrupted by Ctrl-C.
    exec.spin();

    rclcpp::shutdown();

    return 0;
}

Multiple nodes in a single process

The main advantage of the Composition is running multiple nodes in a single process to lower the overhead and use intra-process communication (if it is possible). This tutorial introduces the stereolabs::ZedRgbCvtComponent component and illustrates how to load it with stereolabs::ZedCamera in the same process.

BGRA to BGR conversion

The ZED SDK publishes video frames with 4 channels (BGRA) generating images encoded with 32bit per pixel. This choice allows copying image data from GPU to CPU in a very efficient way. The dark side of this setting is that most of the world works with 24bit images on 3 channels and ROS belongs to this world.

Converting all the retrieved images from 32 bit to 24bit would be a very demanding process, so the best way to obtain a 24bit image from a ZED camera is to write a filter node that subscribes the 32bit topic, converts it to 24bit and publishes the new converted image.

The new node is defined in the zed_rgb_convert package in the zed-ros2-examples repository.

The code explained

The stereolabs::ZedRgbCvtComponent component

The stereolabs::ZedRgbCvtComponent component is defined in the files zed_rgb_convert_component.hpp and zed_rgb_convert_component.cpp.

stereolabs::ZedRgbCvtComponent class declaration:

namespace stereolabs {

class ZedRgbCvtComponent : public rclcpp::Node
{

public:
    ZED_CVT_COMPONENT_PUBLIC
    explicit ZedRgbCvtComponent(const rclcpp::NodeOptions & options);

    virtual ~ZedRgbCvtComponent(){}

protected:
    void camera_callback(const sensor_msgs::msg::Image::ConstSharedPtr& img,
                         const sensor_msgs::msg::CameraInfo::ConstSharedPtr& cam_info);

private:
    // Publisher
    image_transport::CameraPublisher mPubBgr;

    // Subscriber
    image_transport::CameraSubscriber mSubBgra;

    // QoS parameters
    rclcpp::QoS mVideoQos;
};
}

The stereolabs::ZedRgbCvtComponent object inherits from rclcpp::Node. A constructor, an image callback and three members are defined.

camera_callback is the function called each time a new image and the relative camera data are received.
mPubBgr is the image_transport::CameraPublisher object that publishes the converted image
mSubBgra is the image_transport::CameraSubscriber object that receives the original image and the relative camera data
mVideoQos are the Quality of Service parameters for the subscriber and the publisher

ZedRgbCvtComponent::ZedRgbCvtComponent(const rclcpp::NodeOptions &options)
    : Node("zed_cvt_node", options)
    , mVideoQos(1) {

    RCLCPP_INFO(get_logger(), "********************************");
    RCLCPP_INFO(get_logger(), " ZED BGRA2BGA Convert Component ");
    RCLCPP_INFO(get_logger(), "********************************");
    RCLCPP_INFO(get_logger(), " * namespace: %s", get_namespace());
    RCLCPP_INFO(get_logger(), " * node name: %s", get_name());
    RCLCPP_INFO(get_logger(), "********************************");

    mVideoQos.keep_last(10);
    mVideoQos.best_effort();
    mVideoQos.durability_volatile();

    // Create camera publisher for converted image topic
    mPubBgr = image_transport::create_camera_publisher( this, "~/zed_image_3ch", mVideoQos.get_rmw_qos_profile() );
    RCLCPP_INFO_STREAM( get_logger(), "Advertised on topic: " << mPubBgr.getTopic());

    // Create camera subscriber
    mSubBgra = image_transport::create_camera_subscription( this,
                                                 "zed_image_4ch",
                                                 std::bind(&ZedRgbCvtComponent::camera_callback,this,_1,_2),
                                                 "raw",
                                                 mVideoQos.get_rmw_qos_profile());

    RCLCPP_INFO_STREAM( get_logger(), "Subscribed on topic: " << mSubBgra.getTopic());
    RCLCPP_INFO_STREAM( get_logger(), "Subscribed on topic: " << mSubBgra.getInfoTopic());
}

In the constructor we initially publish a log with info relative to namespace and name of the node, than we define the QoS parameters, the image publisher and the image subscriber. Like in the tutorials seen in the previous sections both publisher and subscribers use generic topic names (i.e. zed_image_3ch and zed_image_4ch), that require a remapping when starting the node. The default name chosen for the node is zed_cvt_node.

void ZedRgbCvtComponent::camera_callback(const sensor_msgs::msg::Image::ConstSharedPtr& img,
                                         const sensor_msgs::msg::CameraInfo::ConstSharedPtr& cam_info)
{
    // Check for correct input image encoding
    if(img->encoding!=sensor_msgs::image_encodings::BGRA8) {
        RCLCPP_ERROR(get_logger(), "The input topic image requires 'BGRA8' encoding");
        exit(EXIT_FAILURE);
    }

    // Convert BGRA to BGR using OpenCV
    cv::Mat bgra(img->height,img->width,CV_8UC4,(void*)(&img->data[0]));
    cv::Mat bgr;

    cv::cvtColor(bgra,bgr,cv::COLOR_BGRA2BGR);

    // Create the output message and copy converted data
    std::shared_ptr<sensor_msgs::msg::Image> out_bgr = std::make_shared<sensor_msgs::msg::Image>();

    out_bgr->header.stamp = img->header.stamp;
    out_bgr->header.frame_id = img->header.frame_id;
    out_bgr->height = bgr.rows;
    out_bgr->width = bgr.cols;

    int num = 1; // for endianness detection
    out_bgr->is_bigendian = !(*(char*)&num == 1);

    out_bgr->step = bgr.step;

    size_t size = out_bgr->step * out_bgr->height;
    out_bgr->data.resize(size);

    out_bgr->encoding = sensor_msgs::image_encodings::BGR8;
    memcpy((char*)(&out_bgr->data[0]), &bgr.data[0], size);

    // Publish the new image message coupled with camera info from the original message
    mPubBgr.publish( out_bgr, cam_info);
}

The image callback first of all checks if the received image has the correct encoding, since we want a BGRA8 image as input. The received image is converted from BGRA (CV_8UC4) to BGR (CV_8UC4) using the function cvtColor of the OpenCV library. After the conversion the output topic is created, all the fields are correctly initialized and finally it is published together with the original camera information.

The zed_cvt_node container`

To start the conversion node together with the ZED node we create a rclcpp::executors::MultiThreadedExecutor container that takes care of running both the components in the same process:

int main(int argc, char * argv[])
{
    // Force flush of the stdout buffer.
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);

    // Initialize any global resources needed by the middleware and the client library.
    // This will also parse command line arguments one day (as of Beta 1 they are not used).
    // You must call this before using any other part of the ROS system.
    // This should be called once per process.
    rclcpp::init(argc, argv);

    // Create an executor that will be responsible for execution of callbacks for a set of nodes.
    // With this version, all callbacks will be called from within this thread (the main one).
    rclcpp::executors::MultiThreadedExecutor exec;
    rclcpp::NodeOptions options;

    // Enable intraprocess communication
    options.use_intra_process_comms(true); 

    // Add ZedCamera node
    auto zed_node = std::make_shared<stereolabs::ZedCamera>(options);
    exec.add_node(zed_node);

    // Add ZedRgbCvtComponent node
    auto zed_cvt_node = std::make_shared<stereolabs::ZedRgbCvtComponent>(options);
    exec.add_node(zed_cvt_node);

    // spin will block until work comes in, execute work as it becomes available, and keep blocking.
    // It will only be interrupted by Ctrl-C.
    exec.spin();

    rclcpp::shutdown();

    return 0;
}

After the standard node initialization a rclcpp::executors::MultiThreadedExecutor is created and nodes options are defined.

It is important to notice that intra-process communication is enabled, this allows to take advantage of the zero-copy feature and reduce the communication overhead between the ZED Camera component and the RGB Convert component (see Intra Process Communication).

Next the ZedCamera node is created and added to the container, the same is done for the ZedRgbCvtComponent node.

Finally the container can start the execution of both the nodes waiting for a stop by the user pressing Ctrl+C.

The launch file

Starting the node using the $ ros2 run zed_rgb_convert zed_rgb_convert CLI command will result in a failure: many errors and warning will be logged in the console, while publishers and subscribers will be not linked.

To correctly start the nodes in their container a launch file is required, where parameters for the zed_node are correctly loaded and topics names are correctly remapped:

import os

from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import Command


def generate_launch_description():
    # Define LaunchDescription variable
    ld = LaunchDescription()

    # use:
    #  - 'zed' for "ZED" camera
    #  - 'zedm' for "ZED mini" camera
    #  - 'zed2' for "ZED2" camera
    camera_model = 'zed2'

    # Camera name
    camera_name = 'zed2'

    # URDF/xacro file to be loaded by the Robot State Publisher node
    xacro_path = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'urdf', 'zed_descr.urdf.xacro'
    )

    # ZED Configurations to be loaded by ZED Node
    config_common = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'config',
        'common.yaml'
    )

    config_camera = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'config',
        camera_model + '.yaml'
    )

    # Set LOG format
    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time} [{name}] [{severity}] {message}'

    # Robot State Publisher node
    rsp_node = Node(
        package='robot_state_publisher',
        namespace=camera_name,
        executable='robot_state_publisher',
        name='zed_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': Command(
                [
                    'xacro', ' ', xacro_path, ' ',
                    'camera_name:=', camera_name, ' ',
                    'camera_model:=', camera_model
                ])
        }]
    )

    # ZED node using manual composition
    zed_node = Node(
        package='zed_rgb_convert',
        namespace=camera_name,
        executable='zed_rgb_convert',
        output='screen',
        parameters=[
            config_common,  # Common parameters
            config_camera,  # Camera related parameters
        ],
        remappings=[
            ("zed_image_4ch", "zed_node/rgb/image_rect_color"),
            ("camera_info", "zed_node/rgb/camera_info")
        ]
    )

    # Add nodes to LaunchDescription
    ld.add_action(rsp_node)
    ld.add_action(zed_node)

    return ld

Let’s analyze it in details.

First of all the required modules and functions are imported:

import os

from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import Command

then a LaunchDescription object is defined to initialize and start the node.

def generate_launch_description():
    # Define LaunchDescription variable
    ld = LaunchDescription()

It is required to define the model of the camera used and give it a name:

    # use:
    #  - 'zed' for "ZED" camera
    #  - 'zedm' for "ZED mini" camera
    #  - 'zed2' for "ZED2" camera
    camera_model = 'zed2'

    # Camera name
    camera_name = 'zed2'

Next we must create the paths to the xacro file that must be processed to create the camera description URDF file and to the parameter files contained in the zed_wrapper package. The get_package_share_directory is an useful function that returns the full path of the shared directory of each package starting from its name. The correct paths are then created using the model of the camera defined above:

    # URDF/xacro file to be loaded by the Robot State Publisher node
    xacro_path = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'urdf', 'zed_descr.urdf.xacro'
    )

    # ZED Configurations to be loaded by ZED Node
    config_common = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'config',
        'common.yaml'
    )

    config_camera = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'config',
        camera_model + '.yaml'
    )

A custom format is defined for the console LOG information, adding timestamps and node names before the LOG message:

    # Set LOG format
    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time} [{name}] [{severity}] {message}'

A robot_state_publisher node is required to correctly run the zed_node. It defines all the static frames that compose the ZED cameras starting from the URDF file that is created by the xacro command called with the parameters camera_name and camera_model:

# Robot State Publisher node
    rsp_node = Node(
        package='robot_state_publisher',
        namespace=camera_name,
        executable='robot_state_publisher',
        name='zed_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': Command(
                [
                    'xacro', ' ', xacro_path, ' ',
                    'camera_name:=', camera_name, ' ',
                    'camera_model:=', camera_model
                ])
        }]
    )

Now we can define the container node that runs the zed_node and the zed_cvt_node as created above in C++. Here all the parameters are correctly initialized and set: node_namespace from the camera name defined above, the node parameters from the YAML files which path has been constructed above and finally the fundamental topics remapping to correctly link publishers and subscribers:

# ZED node using manual composition
    zed_node = Node(
        package='zed_rgb_convert',
        namespace=camera_name,
        executable='zed_rgb_convert',
        output='screen',
        parameters=[
            config_common,  # Common parameters
            config_camera,  # Camera related parameters
        ],
        remappings=[
            ("zed_image_4ch", "zed_node/rgb/image_rect_color"),
            ("camera_info", "zed_node/rgb/camera_info")
        ]
    )

Finally zed_node and rsp_node are added to the launch descriptor that will be passed to the launch server to correctly start all the nodes:

    # Add nodes to LaunchDescription
    ld.add_action(rsp_node)
    ld.add_action(zed_node)

    return ld

The result is illustrated in the following node graph for a ZED2 camera:

The zed_node and the zed_cvt_node both belong to the /zed2 namespace. The zed_node publishes rgb/camera_info and rgb/image_rect_color that are subscribed by the zed_cvt_node. After conversion the zed_cvt_node publishes the zed_image_3ch topic that is finally subscribed by a rqt viewer to display the image stream.