Depth Sensing

Depth with RVIZ

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
  • Unreliable: Sets the QOS profile to BEST_EFFORT if checked
  • Queue size: The size of the message queue. Use a lower value to reduce latency
  • 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)

You can change the rendering style and the color transformer as explained above for the Depthcloud plugin.

Confidence

To visualize the Confidence Map as an image, you can use the Image plugin subscribing to the topic /zed/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

Use the following command to connect the ZED camera to the ROS network:

$ ros2 launch stereolabs_zed zed.launch.py

The ZED node starts to publish depth data to 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 just compiled and you can run the subscriber node using this command:

$ ros2 run stereolabs_zed_tutorial_depth zed_depth_tutorial

If the ZED node is running, and a ZED or ZED-M 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_sub]: Center distance : 1.09941 m
[INFO] [zed_depth_sub]: Center distance : 1.09939 m
[INFO] [zed_depth_sub]: Center distance : 1.09978 m
[INFO] [zed_depth_sub]: Center distance : 1.09962 m
[INFO] [zed_depth_sub]: Center distance : 1.09941 m
[INFO] [zed_depth_sub]: Center distance : 1.09979 m
[INFO] [zed_depth_sub]: Center distance : 1.09974 m
[INFO] [zed_depth_sub]: Center distance : 1.09967 m
[INFO] [zed_depth_sub]: Center distance : 1.09958 m
[INFO] [zed_depth_sub]: Center distance : 1.09947 m
[INFO] [zed_depth_sub]: Center distance : 1.09925 m

[...]

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_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.
         */

        mSub = create_subscription<sensor_msgs::msg::Image>(
                   "/zed/zed_node/depth/depth_registered",
                   std::bind(&MinimalDepthSubscriber::depthCallback, this, _1),
                   camera_qos_profile);
    }

  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 mSub;
};

// 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 capatibilities.

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_sub") {
        
        rmw_qos_profile_t camera_qos_profile = rmw_qos_profile_sensor_data;

        mSub = create_subscription<sensor_msgs::msg::Image>(
                   "/zed/zed_node/depth/depth_registered",
                   std::bind(&MinimalDepthSubscriber::depthCallback, this, _1),
                   camera_qos_profile);
    }

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

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

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

It is also important to define the right QOS profile to be sure that the topics are correctly received. As explained in the ZED node guide, in ROS2 the publishers and the subscribers must use a compatible QOS profile. In this case, we use the pre defined rmw_qos_profile_sensor_data as done with the depth publisher of the ZED node.

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 /zed/zed_node/depth/depth_registered. 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 node’s code 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.