> For clean Markdown of any page, append .md to the page URL. > For a complete documentation index, see https://docs.stereolabs.com/llms.txt. > For AI client integration (Claude Code, Cursor, etc.), connect to the MCP server at https://docs.stereolabs.com/_mcp/server. # Using Positional Tracking with ROS 2 ## Configuring the positional tracking All the positional tracking parameters live in the `pos_tracking` namespace of `common_stereo.yaml`. See the [Positional tracking parameters](/docs/integrations/ros-2/zed-stereo-node/#positional-tracking-parameters-odometry-and-localization) reference for the full list. Two of them drive the algorithm itself: * `pos_tracking.pos_tracking_mode`: `AUTO` uses the ZED SDK default. Use `GEN_1` for the legacy behavior and `GEN_3` for the latest algorithms. * `pos_tracking.compute_preference` \[ZED SDK >= 5.5]: how much GPU the positional tracking may use. `PREFER_GPU` makes the tracking faster and shortens `grab()`, `PREFER_CPU` keeps the GPU free for your own workloads. `GEN_3` runs on the CPU by default, while `GEN_1` computes depth and therefore uses the GPU regardless of the setting. ### Pose confidence With ZED SDK >= 5.4 and `POSITIONAL_TRACKING_MODE::GEN_3`, the node reports a `[0,100]` quality metric for the pose, where `0` means that tracking is lost. It is published as the `pose_confidence` field of the [`PosTrackStatus`](/docs/integrations/ros-2/custom-msgs/#positional-tracking-status) message on `~/pose/status`, and shown as `Pose confidence` in the [node diagnostic](/docs/integrations/ros-2/zed-stereo-node/#node-diagnostic). It requires `zed_msgs` v5.5 and stays at `-1` until the first pose is retrieved. > **Note** > > When positional tracking is disabled (`pos_tracking.pos_tracking_enabled: false`) the node does not advertise the `odom` and `pose` topics, does not publish TF, and does not advertise the positional tracking services. The GNSS fusion and the Plane Detection are automatically disabled as well. ## Position with RViz 2 In this tutorial, you will learn in detail how to configure your own RViz 2 session to see only the position data information that you require. RViz 2 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`. ![image](/_fern-img/9a4f4fa067de8ca925a8c8fa495cf496aed0e853723fcf374a6f254e69d17788.webp) Key parameters: * `Topic`: Selects the pose topic `/zed/zed_node/pose` * `Reliability Policy`: Set the QoS reliability policy. `Best Effort` is suggested for performance, `Reliable` is used for compatibility * `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 pose 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 ![image](/_fern-img/92725ccdf8ba0b19bf1687730005b55093e61447d95d53b46b04d5575771b193.webp) Above you can see both the `Pose` (green) and the `Odometry` (red) paths. The `odometry pose` is calculated with a visual-inertial odometry algorithm (pure visual odometry on cameras without an IMU) as the sum of the movement from one step to the next. It is therefore affected by drift. The `camera pose` is instead continuously corrected using the StereoLabs tracking algorithm that combines visual information, space memory information, and, on cameras equipped with an IMU (e.g. ZED Mini, ZED 2, ZED 2i, ZED X, ZED X Mini), 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: ```bash ros2 launch zed_display_rviz2 display_zed_cam.launch.py camera_model:= ``` > **Note** > > The old launch command `ros2 launch zed_display_rviz2 display_.launch.py` is obsolete. Use the `display_zed_cam.launch.py` command above. The ZED node will start to publish pose 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](https://github.com/stereolabs/zed-ros2-examples#build-the-package), the executable of this tutorial has been compiled and you can run the subscriber node using this command: ```bash 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: ```bash 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: ```bash [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`: **`C++`** ```cpp C++ #include #include #include #include #include #include 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( "pose", qos, std::bind(&MinimalPoseOdomSubscriber::poseCallback, this, _1) ); // Create odom subscriber mOdomSub = create_subscription( "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::SharedPtr mPoseSub; rclcpp::Subscription::SharedPtr mOdomSub; }; int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); 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](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Composition.html) 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, `zed_odom_pose_tutorial`: **`C++`** ```cpp C++ 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( "pose", qos, std::bind(&MinimalPoseOdomSubscriber::poseCallback, this, _1) ); // Create odom subscriber mOdomSub = create_subscription( "odom", qos, std::bind(&MinimalPoseOdomSubscriber::odomCallback, this, _1) ); } ``` The constructor mainly defines `mPoseSub` and `mOdomSub`, two `std::shared_ptr` to objects that create two subscribers in the node. The subscribers listen to the `pose` and `odom` topics and call the callback functions `MinimalPoseOdomSubscriber::poseCallback` and `MinimalPoseOdomSubscriber::odomCallback` every time they receive a message. 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(&MinimalPoseOdomSubscriber::, 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](/docs/integrations/ros-2/zed-stereo-node/#qos-profiles). Now we must define the callbacks to execute when one of the subscribed topics is received: **`C++`** ```cpp C++ 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/PoseStamped` and `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. **`C++`** ```cpp C++ 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 the `x`, `y`, and `z` fields of a `geometry_msgs/Point`. **`C++`** ```cpp C++ // 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. **`C++`** ```cpp C++ // 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); ``` We can then print the received information and the timestamp of the message to the screen, after converting the radian values to degrees. Finally, the `main` function is standard for a ROS 2 node that executes a single component: **`C++`** ```cpp C++ int main(int argc, char* argv[]) { rclcpp::init(argc, argv); auto pos_track_node = std::make_shared(); 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\_pose\_tutorial](https://github.com/stereolabs/zed-ros2-examples/tree/master/tutorials/zed_pose_tutorial) sub-package. Along with the node source code, you can find the `package.xml` and `CMakeLists.txt` files that complete the tutorial package.