ROS 2 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 information 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 information
  • 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 pusblisher 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 coverted 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;
    options.use_intra_process_comms(false);

    // 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 disabled, contrary to what one would expect. It would be very important for this kind of application, but we cannot enable it. The motivation is linked to the mStaticTfBroadcaster object defined in the ZedCamera component. Every Static TF Broadcaster uses “Transient Local” as setting for the “durability” parameter of its QoS setting and intra-process communication instead requires that all the publishers and subscribers use “Volatile” as durability setting.

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 correcly 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


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 file to be loaded by Robot State Publisher
    urdf = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'urdf', camera_model + '.urdf'
    )

    # 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',
        node_namespace="/"+camera_name,
        node_executable='robot_state_publisher',
        node_name=camera_name+'_state_publisher',
        output='screen',
        arguments=[urdf],
    )

    # ZED node using manual composition
    zed_node = Node(
        package='zed_rgb_convert',
        node_namespace="/"+camera_name,
        node_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 analize 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

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 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 file to be loaded by Robot State Publisher
    urdf = os.path.join(
        get_package_share_directory('zed_wrapper'),
        'urdf', camera_model + '.urdf'
    )

    # 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:

  # Robot State Publisher node
    rsp_node = Node(
        package='robot_state_publisher',
        node_namespace="/"+camera_name,
        node_executable='robot_state_publisher',
        node_name=camera_name+'_state_publisher',
        output='screen',
        arguments=[urdf],
    )

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',
        node_namespace="/"+camera_name,
        node_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.