Adding Depth Perception in ROS2

Depth with RVIZ2

In this tutorial, you will learn in detail how to configure your own RVIZ session to see only the depth data that you require.

Depth information can be visualized in many different ways: 2D depth images, 3D point clouds, 3D registered depth clouds, and the confidence image to name a few.

Depth image

To visualize a depth image, you can use the simple Image plugin since the depth data are published on topics of type sensor_msgs/Image. It differs from a “normal” image in that the data is encoded in 32-bit (floating point) not 8-bit.

The parameters are the same as for Image (see the Video tutorial) with three additions:

• Normalize range: Since a floating point image is not directly rendered, it is converted to an 8-bit grayscale image. Enabling this field means the normalization range is automatically calculated
• Min value: If Normalize range is unchecked, you can manually set the minimum depth range in meters
• Max value: If Normalize range is unchecked, you can manually set the maximum depth range in meters

Manually setting the normalization range is useful if you know the maximum value measured for the depth and you want to keep the image scale static.

Pointcloud

To directly visualize a topic of type sensor_msgs/Pointcloud2 you can use the Pointcloud2 plugin.

Key parameters:

• Topic: Selects the topic to visualize from the list of available point cloud messages
• 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, Reliable is used for compatibility
• Durability Policy: Set the QoS durability policy. Volatile is suggested for compatibility
• Style: The style used to render each point. Use Points and Size=1 to maximize FPS
• Color transformer: Selects how points are colored. Use RGB8 to match the color image pixels to the corresponding depth pixels. Use Axis color to use a color proportional to the value of the Axis value (e.g. Z to use a color scale proportional to the distance from the floor or X to use a color proportional to the distance from the camera)

Note: if mapping is enabled (see mapping/mapping_enabled parameter), the Pointcloud2 plugin is used also to visualize the fused point cloud result of the mapping elaboration, subscribing to the topic /zed/zed_node/point_cloud/fused_cloud_registered.

Confidence

To visualize the Confidence Map as an image, you can use the Image plugin subscribing to the topic ~/confidence/confidence_image.

The lighter a pixel, the more reliable its corresponding depth value is.

Depth 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 in order to retrieve depth images published by the ZED node and to get the measured distance at the center of the image.

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

ZED2i:

$ ros2 launch zed_wrapper zed2i.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 zed_tutorial_depth zed_tutorial_depth 

The tutorial node subscribes generic depth topic, so a remapping is required to connect to the correct topic published by the ZED node:

ZED:

$ ros2 run zed_tutorial_depth zed_tutorial_depth --ros-args -r depth:=/zed/zed_node/depth/depth_registered

ZED-M:

$ ros2 run zed_tutorial_depth zed_tutorial_depth --ros-args -r depth:=/zedm/zed_node/depth/depth_registered

ZED2:

$ ros2 run zed_tutorial_depth zed_tutorial_depth --ros-args -r depth:=/zed2/zed_node/depth/depth_registered

ZED2i:

$ ros2 run zed_tutorial_depth zed_tutorial_depth --ros-args -r depth:=/zed2i/zed_node/depth/depth_registered

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 your are correctly subscribing to the ZED image topics:

[INFO] [zed_depth_tutorial]: Center distance : 0.981005 m
[INFO] [zed_depth_tutorial]: Center distance : nan m
[INFO] [zed_depth_tutorial]: Center distance : nan m
[INFO] [zed_depth_tutorial]: Center distance : 1.20664 m
[INFO] [zed_depth_tutorial]: Center distance : 1.16232 m
[INFO] [zed_depth_tutorial]: Center distance : 1.15096 m
[INFO] [zed_depth_tutorial]: Center distance : 1.15708 m
[INFO] [zed_depth_tutorial]: Center distance : 1.14709 m
[INFO] [zed_depth_tutorial]: Center distance : 1.15806 m
[INFO] [zed_depth_tutorial]: Center distance : 1.16716 m
[INFO] [zed_depth_tutorial]: Center distance : 1.17654 m
[INFO] [zed_depth_tutorial]: Center distance : 1.16571 m
[INFO] [zed_depth_tutorial]: Center distance : 1.16145 m


[...]

Note: nan values for depths are reported if the central pixel of the image does not contain a valid depth measure (occlusion, flare, low textured surface, …).

The code

The source code of the subscriber node zed_depth_sub_tutorial.cpp:

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

using namespace std::placeholders;

class MinimalDepthSubscriber : public rclcpp::Node {
  public:
    MinimalDepthSubscriber()
        : Node("zed_depth_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 depth_qos(10);
        depth_qos.keep_last(10);
        depth_qos.best_effort();
        depth_qos.durability_volatile();

        // Create depth map subscriber
        mDepthSub = create_subscription<sensor_msgs::msg::Image>(
                   "depth", depth_qos,
                   std::bind(&MinimalDepthSubscriber::depthCallback, this, _1) );
    }

  protected:
    void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
        // Get a pointer to the depth values casting the data
        // pointer to floating point
        float* depths = (float*)(&msg->data[0]);

        // Image coordinates of the center pixel
        int u = msg->width / 2;
        int v = msg->height / 2;

        // Linear index of the center pixel
        int centerIdx = u + msg->width * v;

        // Output the measure
        RCLCPP_INFO(get_logger(), "Center distance : %g m", depths[centerIdx]);
    }

  private:
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr mDepthSub;
};

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

    auto depth_node = std::make_shared<MinimalDepthSubscriber>();

    rclcpp::spin(depth_node);
    rclcpp::shutdown();
    return 0;
}

The code explained

The tutorial is written using the new concept of Component introduced in ROS2 in order to take advantage of the node composition capabilities.

A component named MinimalDepthSubscriber is created subclassing the ROS2 object rclcpp::Node.

In the constructor, we initialize the parent class Node with the name of our node new zed_depth_sub:

    MinimalDepthSubscriber()
        : Node("zed_depth_tutorial") {
        rclcpp::QoS depth_qos(10);
        depth_qos.keep_last(10);
        depth_qos.best_effort();
        depth_qos.durability_volatile();

        // Create depth map subscriber
        mDepthSub = create_subscription<sensor_msgs::msg::Image>(
                   "depth", depth_qos,
                   std::bind(&MinimalDepthSubscriber::depthCallback, this, _1) );
    }

The constructor mainly defines mDepthSub, a std::SharedPtr to an object that creates a subscriber in the node. The subscriber checks for topics of type depth and recalls the callback function MinimalDepthSubscriber::depthCallback every time it receives one of it.

ROS2 uses the new constructs available with C++11 and C++14, so to bind the callback (a class method in this case) to the mDepthSub object we use the form

std::bind(&MinimalDepthSubscriber::depthCallback, this, _1).

It is important that the subscription 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

Now we must define the callback to execute when one of the subscribed topics is received:

    void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
        // Get a pointer to the depth values casting the data
        // pointer to floating point
        float* depths = (float*)(&msg->data[0]);

        // Image coordinates of the center pixel
        int u = msg->width / 2;
        int v = msg->height / 2;

        // Linear index of the center pixel
        int centerIdx = u + msg->width * v;

        // Output the measure
        RCLCPP_INFO(get_logger(), "Center distance : %g m", depths[centerIdx]);
    }

This callback is executed when the subscriber node receives a message of type sensor_msgs/Image that matches the subscribed topic depth. The parameter of the callback is a std::shared_ptr to the received message. This means that you don’t have to worry about memory management for it.

The callback demonstrates how to access the message data:

• The pointer to the data field is casted to float* as it is declared as a pointer to an array of char values
• The image coordinates [u,v] of the center of the depth image are calculated
• Since the data are organized in a linear vector, the index of the central pixel is calculated from the image coordinates
• The measure in meters of the center point is printed on screen

Finally, the main function is standard for a ROS2 node that executes a single component:

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

    auto depth_node = std::make_shared<MinimalDepthSubscriber>();

    rclcpp::spin(depth_node);
    rclcpp::shutdown();
    return 0;
}

The ROS2 environment is initialized using the rclcpp::init command. Then we create a depth_node Component as a std::shared_ptr. to the MinimalDepthSubscriber class that we defined above.

Then the code of the node is executed in the main thread using the rclcpp::spin(depth_node); command. The node will run until the user presses Ctrl+C to stop it, shut down, and exit.

Conclusions

The full source code of this tutorial is available on GitHub in the zed_depth_tutorial sub-package.

Along with the node source code, you can find the package.xml and CMakeLists.txt files that complete the tutorial package.