Adding Positional Tracking in ROS2 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 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 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_pos_tracking zed_tutorial_pos_tracking The tutorial node subscribes generic pose and odom topics, so a remapping is required to connect to the correct topics published by the ZED node: ZED: $ ros2 run zed_tutorial_pos_tracking zed_tutorial_pos_tracking --ros-args -r odom:=/zed/zed_node/odom -r pose:=/zed/zed_node/pose ZED-M: $ ros2 run zed_tutorial_pos_tracking zed_tutorial_pos_tracking --ros-args -r odom:=/zedm/zed_node/odom -r pose:=/zedm/zed_node/pose ZED2: $ ros2 run zed_tutorial_pos_tracking zed_tutorial_pos_tracking --ros-args -r odom:=/zed2/zed_node/odom -r pose:=/zed2/zed_node/pose ZED2i: $ ros2 run zed_tutorial_pos_tracking zed_tutorial_pos_tracking --ros-args -r odom:=/zed2i/zed_node/odom -r pose:=/zed2i/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 your 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 ROS2 in order to take advantage of the node composition capabilities. A component named MinimalPoseOdomSubscriber 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_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 checks for topics of type pose and odom and recalls the callback functions MinimalPoseOdomSubscriber::poseCallback and MinimalPoseOdomSubscriber::odomCallback every time they receive one of them. ROS2 uses the new costructs 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 to 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 ROS2 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 ROS2 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 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.