Adding Positional Tracking in ROS 2

Position with RVIZ #

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

RVIZ provides plugins for visualizing the camera’s pose and its path over time.

Camera pose #

The Pose plugin provides a visualization of the position and orientation of the camera (geometry_msgs/PoseStamped) in the Map frame.

The Topic to be subscribed to is ~/pose`.

Key parameters:

Topic: Selects the odometry topic /zed/zed_node/odom
Unreliable: Check this to reduce the latency of the messages
Position tolerance and Angle tolerance: set both to 0 to be sure to visualize all positional data
Keep: Set to the number of messages to visualize at a time
Shape: You can visualize odometry information as an arrow pointing in the direction of the X axis, or as a three Axes frame. Both shapes can be resized to your preference

Camera path #

The ZED wrapper provides two different paths for the camera position and orientation:

~/path_map: The history of the camera pose in Map frame
~/path_odom: The history of the odometry of the camera in Map frame

Above you can see both the Pose (green) and the Odometry (red) paths.

The odometry pose is calculated with a pure “visual odometry” algorithm as the sum of the movement from one step to the next. It is therefore affected by drift.

The camera pose is instead continuously fixed using the Stereolabs tracking algorithm that combines visual information, space memory information and if using a “ZED-M” or a “ZED2”, inertial information.

The parameters to be configured are analogous to the parameters seen above for the Pose plugin.

Position info subscribing in C++ #

In this tutorial, you will learn how to write a simple C++ node that subscribes to messages of type geometry_msgs/PoseStamped and nav_msgs/Odometry to retrieve the position and the orientation of the ZED camera in the Map and in the Odometry frames.

Introduction #

Open a new console and use this command to connect the camera to the ROS 2 network:

ros2 launch zed_display_rviz2 display_zed_cam.launch.py camera_model:=<camera model>

Note: the old launch command $ ros2 launch zed_display_rviz2 display_<camera model>.launch.py is now obsolete and will be removed in a future release of the wrapper.

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 ROS 2 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_pos_tracking zed_tutorial_pos_tracking

The tutorial node subscribes to generic pose and odom topics, so a remapping is required to connect to the correct topics published by the ZED node:

$ ros2 run zed_tutorial_pos_tracking zed_tutorial_pos_tracking --ros-args -r odom:=/zed/zed_node/odom -r pose:=/zed/zed_node/pose

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 you are correctly subscribing to the ZED positional tracking topics:

[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.61113293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: -0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.61113293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.161133293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: -0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.161133293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.261042293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: -0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.261042293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.361011293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: -0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.361011293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.461130293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: -0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.461130293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.561097293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: 0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.561097293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: 0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.661202293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.661202293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: 0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.761130293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.761130293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: 0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.861144293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.861144293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: 0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116715.961162293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116715.961162293 sec 
[INFO] [zed_odom_pose_tutorial]: Received odometry in 'odom' frame : X: 0.00 Y: 0.00 Z: 0.00 - R: 0.28 P: 5.68 Y: 0.07 - Timestamp: 1603116716.61205293 sec 
[INFO] [zed_odom_pose_tutorial]: Received pose in 'map' frame : X: 0.00 Y: -0.00 Z: -0.00 - R: 0.40 P: 5.67 Y: 0.00 - Timestamp: 1603116716.61205293 sec

The code #

The source code of the subscriber node zed_tutorial_pos_tracking.cpp:

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

using namespace std::placeholders;

#define RAD2DEG 57.295779513

class MinimalPoseOdomSubscriber : public rclcpp::Node {
public:
    MinimalPoseOdomSubscriber()
        : Node("zed_odom_pose_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 qos(10);
        qos.keep_last(10);
        qos.best_effort();
        qos.durability_volatile();

        // Create pose subscriber
        mPoseSub = create_subscription<geometry_msgs::msg::PoseStamped>(
                    "pose", qos,
                    std::bind(&MinimalPoseOdomSubscriber::poseCallback, this, _1) );

        // Create odom subscriber
        mOdomSub = create_subscription<nav_msgs::msg::Odometry>(
                    "odom", qos,
                    std::bind(&MinimalPoseOdomSubscriber::odomCallback, this, _1) );
    }

protected:
    void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        // Camera position in map frame
        double tx = msg->pose.position.x;
        double ty = msg->pose.position.y;
        double tz = msg->pose.position.z;

        // Orientation quaternion
        tf2::Quaternion q(
                    msg->pose.orientation.x,
                    msg->pose.orientation.y,
                    msg->pose.orientation.z,
                    msg->pose.orientation.w);

        // 3x3 Rotation matrix from quaternion
        tf2::Matrix3x3 m(q);

        // Roll Pitch and Yaw from rotation matrix
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);

        // Output the measure
        RCLCPP_INFO(get_logger(), "Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
                 msg->header.frame_id.c_str(),
                 tx, ty, tz,
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
    }

    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
        // Camera position in map frame
        double tx = msg->pose.pose.position.x;
        double ty = msg->pose.pose.position.y;
        double tz = msg->pose.pose.position.z;

        // Orientation quaternion
        tf2::Quaternion q(
                    msg->pose.pose.orientation.x,
                    msg->pose.pose.orientation.y,
                    msg->pose.pose.orientation.z,
                    msg->pose.pose.orientation.w);

        // 3x3 Rotation matrix from quaternion
        tf2::Matrix3x3 m(q);

        // Roll Pitch and Yaw from rotation matrix
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);

        // Output the measure
        RCLCPP_INFO(get_logger(), "Received odometry in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
                 msg->header.frame_id.c_str(),
                 tx, ty, tz,
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
    }

private:
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr mPoseSub;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr mOdomSub;
};



int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MinimalPoseOdomSubscriber>());
    rclcpp::shutdown();
    return 0;
}

The code explained #

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

A component named MinimalPoseOdomSubscriber is created subclassing the ROS 2 object rclcpp::Node.

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

MinimalPoseOdomSubscriber()
        : Node("zed_odom_pose_tutorial") {
        rclcpp::QoS qos(10);
        qos.keep_last(10);
        qos.best_effort();
        qos.durability_volatile();

        // Create pose subscriber
        mPoseSub = create_subscription<geometry_msgs::msg::PoseStamped>(
                    "pose", qos,
                    std::bind(&MinimalPoseOdomSubscriber::poseCallback, this, _1) );

        // Create odom subscriber
        mOdomSub = create_subscription<nav_msgs::msg::Odometry>(
                    "odom", qos,
                    std::bind(&MinimalPoseOdomSubscriber::odomCallback, this, _1) );
    }

The constructor mainly defines mPoseSub and mOdomSub, a std::SharedPtr to two objects that create two subscribers in the node. The subscribers check for topics of type pose and odom and recall the callback functions MinimalPoseOdomSubscriber::poseCallback and MinimalPoseOdomSubscriber::odomCallback every time they receive one of them.

ROS 2 uses the new constructs available with C++11 and C++14, so to bind the callbacks (a class method in this case) to the mPoseSub and mOdomSub objects we use the form

std::bind(&MinimalDepthSubscriber::<callback_name>, this, _1).

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 is configured to keep the last received 10 messages with “best effort” reliability and “volatile” durability. This configuration is highly compatible with many possible publisher configurations.

For more information about QoS compatibility, refer to the ZED node guide

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

    void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        // Camera position in map frame
        double tx = msg->pose.position.x;
        double ty = msg->pose.position.y;
        double tz = msg->pose.position.z;

        // Orientation quaternion
        tf2::Quaternion q(
                    msg->pose.orientation.x,
                    msg->pose.orientation.y,
                    msg->pose.orientation.z,
                    msg->pose.orientation.w);

        // 3x3 Rotation matrix from quaternion
        tf2::Matrix3x3 m(q);

        // Roll Pitch and Yaw from rotation matrix
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);

        // Output the measure
        RCLCPP_INFO(get_logger(), "Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
                 msg->header.frame_id.c_str(),
                 tx, ty, tz,
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
    }

    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
        // Camera position in map frame
        double tx = msg->pose.pose.position.x;
        double ty = msg->pose.pose.position.y;
        double tz = msg->pose.pose.position.z;

        // Orientation quaternion
        tf2::Quaternion q(
                    msg->pose.pose.orientation.x,
                    msg->pose.pose.orientation.y,
                    msg->pose.pose.orientation.z,
                    msg->pose.pose.orientation.w);

        // 3x3 Rotation matrix from quaternion
        tf2::Matrix3x3 m(q);

        // Roll Pitch and Yaw from rotation matrix
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);

        // Output the measure
        RCLCPP_INFO(get_logger(), "Received odometry in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
                 msg->header.frame_id.c_str(),
                 tx, ty, tz,
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
    }

The two callbacks are very similar; the only difference is that poseCallback receives messages of type geometry_msgs/PoseStampedand odomCallback receives messages of type nav_msgs/Odometry. These are similar but not identical. However, the information extracted by the two topics is the same: camera position and camera orientation.

    double tx = msg->pose.position.x;
    double ty = msg->pose.position.y;
    double tz = msg->pose.position.z;

Extracting the position is straightforward since the data is stored in a vector of three floating-point elements.

    // Orientation quaternion
    tf2::Quaternion q(
        msg->pose.orientation.x,
        msg->pose.orientation.y,
        msg->pose.orientation.z,
        msg->pose.orientation.w);

    // 3x3 Rotation matrix from quaternion
    tf2::Matrix3x3 m(q);

    // Roll Pitch and Yaw from rotation matrix
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

Extracting the orientation is less straightforward as it is published as a quaternion vector. To convert the quaternion to a more readable form, we must first convert it to a 3x3 rotation matrix from which we can finally extract the three values for Roll, Pitch and Yaw in radians.

        // Output the measure
        RCLCPP_INFO(get_logger(), "Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
                 msg->header.frame_id.c_str(),
                 tx, ty, tz,
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
                    msg->header.stamp.sec,msg->header.stamp.nanosec);

Finally, we can print the information received to the screen after converting the radian values to degrees with the timestamp of the message.

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

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    
    auto pos_track_node = std::make_shared<MinimalPoseOdomSubscriber>();

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

The ROS 2 environment is initialized using the rclcpp::init command. Then we create a pos_track_node Component as a std::shared_ptr. to the MinimalPoseOdomSubscriber class that we defined above.

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

Conclusions #

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

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