Adding Positional Tracking in ROS 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 odometry The Odometry plugin provides a clear visualization of the odometry of the camera (nav_msgs/Odometry) in the Map frame. 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 As of ZED SDK v2.6, pose covariance is available if the spatial_memory parameter is set to false in the ZED launch file. You can get a visual estimation of the covariance with the odometry plugin by checking the Covariance option. The position covariance is visualized as an ellipsoid centered in the camera frame. The three orientation covariances are visualized as three 2D ellipses centered on the relative axis. You can change the Scale factors to get a better visualization if the ellipsoid and the ellipses are too big (high covariance) or not visible (low covariance). Camera pose The Pose plugin provides a visualization of the position and orientation of the camera (geometry_msgs/PoseStamped) in the Map frame similar to the Odometry plugin, but the Keep parameter and the Covariance parameter are not available. The Topic to be subscribed is /zed/zed_node/pose. Camera path The ZED wrapper provides two different paths for the camera position and orientation: /zed/zed_node/path_map: The history of the camera pose in Map frame /zed/zed_node/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,” inertial information. The parameters to be configured are analogous to the parameters seen above for the Pose and Odometry plugins. Position 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 Use the following command to connect the ZED camera to the ROS network: ZED: $ roslaunch zed_wrapper zed.launch ZED-M: $ roslaunch zed_wrapper zedm.launch ZED2: $ roslaunch zed_wrapper zed2.launch ZED2i: $ roslaunch zed_wrapper zed2i.launch The ZED node starts to publish messages about its position in the network only if there is another node that subscribes to the relative topic. The ZED wrapper publishes two kinds of positions: odometry: The position calculated as the sum of the movements relative to the previous position. Only the pure visual odometry is used pose: The position calculated relative to the world map. Historical information about the environment is used and Inertial data (if using a ZED-M) are fused to get a better 6 DoF pose The ROS wrapper follows ROS REP105 conventions. Running the tutorial If you properly followed the ROS Installation Guide, the executable of this tutorial has just compiled and you can run the subscriber node using the following command: $ rosrun zed_tracking_sub_tutorial zed_tracking_sub If the ZED node is running, and a ZED or ZED-M is connected or you have loaded and SVO file, you will receive the following stream of messages confirming that your are correctly subscribing to the ZED image topics: [ INFO] [1536571277.284918933]: Received odom in 'odom' frame : X: 0.01 Y: 0.11 Z: 0.27 - R: -59.13 P: 29.17 Y: -38.27 [ INFO] [1536571277.308147447]: Received pose in 'map' frame : X: -0.02 Y: -0.04 Z: 0.03 - R: -3.36 P: -4.32 Y: -12.08 [ INFO] [1536571277.328975670]: Received odom in 'odom' frame : X: 0.03 Y: 0.11 Z: 0.28 - R: -62.26 P: 29.26 Y: -37.74 [ INFO] [1536571277.344163358]: Received pose in 'map' frame : X: -0.03 Y: -0.02 Z: 0.03 - R: -1.21 P: -1.00 Y: -13.21 [ INFO] [1536571277.372976799]: Received odom in 'odom' frame : X: 0.04 Y: 0.12 Z: 0.28 - R: -65.35 P: 27.69 Y: -36.27 [ INFO] [1536571277.375593051]: Received pose in 'map' frame : X: -0.04 Y: -0.01 Z: 0.02 - R: -0.24 P: 1.28 Y: -14.98 [ INFO] [1536571277.408942979]: Received pose in 'map' frame : X: -0.06 Y: -0.01 Z: 0.03 - R: -0.85 P: 2.67 Y: -17.63 [ INFO] [1536571277.416963850]: Received odom in 'odom' frame : X: 0.06 Y: 0.13 Z: 0.29 - R: -68.11 P: 26.79 Y: -33.93 [ INFO] [1536571277.417057266]: Received odom in 'odom' frame : X: 0.07 Y: 0.14 Z: 0.29 - R: -69.27 P: 25.64 Y: -29.91 [ INFO] [1536571277.442988345]: Received pose in 'map' frame : X: -0.05 Y: -0.01 Z: 0.03 - R: -0.85 P: 2.67 Y: -17.63 [ INFO] [1536571277.465031616]: Received odom in 'odom' frame : X: 0.09 Y: 0.16 Z: 0.30 - R: -72.27 P: 24.17 Y: -28.24 [ INFO] [1536571277.476189234]: Received pose in 'map' frame : X: -0.06 Y: -0.00 Z: 0.03 - R: -0.85 P: 2.66 Y: -17.63 [ INFO] [1536571277.476251272]: Received odom in 'odom' frame : X: 0.10 Y: 0.18 Z: 0.31 - R: -75.13 P: 23.33 Y: -25.71 [ INFO] [1536571277.509004649]: Received pose in 'map' frame : X: -0.07 Y: -0.01 Z: 0.04 - R: -2.80 P: 5.82 Y: -17.74 [ INFO] [1536571277.516935704]: Received odom in 'odom' frame : X: 0.11 Y: 0.19 Z: 0.31 - R: -73.16 P: 20.36 Y: -20.36 [ INFO] [1536571277.543752689]: Received pose in 'map' frame : X: -0.08 Y: -0.02 Z: 0.06 - R: -4.38 P: 7.86 Y: -17.73 [ INFO] [1536571277.561048985]: Received odom in 'odom' frame : X: 0.11 Y: 0.19 Z: 0.31 - R: -70.84 P: 16.76 Y: -14.78 [ INFO] [1536571277.575540826]: Received pose in 'map' frame : X: -0.09 Y: -0.02 Z: 0.07 - R: -5.79 P: 9.17 Y: -17.61 [ INFO] [1536571277.605050948]: Received odom in 'odom' frame : X: 0.10 Y: 0.20 Z: 0.32 - R: -68.37 P: 12.53 Y: -9.49 [...] If you move your camera by hand, you will see how the position and orientations are updated in real-time, and how odom and pose will drift one by the other due to the fact that odom pose is pure odometry data and is not fixed. The code #include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <nav_msgs/Odometry.h> #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Matrix3x3.h" #include <string> #define RAD2DEG 57.295779513 /** * This tutorial demonstrates receiving ZED odom and pose messages over the ROS system. */ /** * Subscriber callbacks */ void odomCallback(const nav_msgs::Odometry::ConstPtr& 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 ROS_INFO("Received odom in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f", msg->header.frame_id.c_str(), tx, ty, tz, roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); } void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& 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 ROS_INFO("Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f", msg->header.frame_id.c_str(), tx, ty, tz, roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); } /** * Node main function */ int main(int argc, char** argv) { // Node initialization ros::init(argc, argv, "zed_tracking_subscriber"); ros::NodeHandle n; // Topic subscribers ros::Subscriber subOdom = n.subscribe("/zed/zed_node/odom", 10, odomCallback); ros::Subscriber subPose = n.subscribe("/zed/zed_node/pose", 10, poseCallback); // Node execution ros::spin(); return 0; } The code explained The following is a brief explanation about the above source code. void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& 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 ROS_INFO("Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f", msg->header.frame_id.c_str(), tx, ty, tz, roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); } 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. ROS_INFO("Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f", msg->header.frame_id.c_str(), tx, ty, tz, roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); Finally, we can print the information received to the screen after converting the radian values to degrees. The main function is very standard and is explained in detail in the “Talker/Listener” ROS tutorial. It is important to note how the subscribers are defined: // Subscribers ros::Subscriber subOdom = n.subscribe("/zed/zed_node/odom", 10, odomCallback); ros::Subscriber subPose = n.subscribe("/zed/zed_node/pose", 10, poseCallback); A ros::Subscriber is a ROS object that listens on the network and waits for its own topic message to be available. When a message is received, it executes the callback assigned to it. In this tutorial, we declared two subscribers to the pose data: The subscriber to the topic /zed/zed_node/odom calls the odomCallback function when it receives a message of type nav_msgs/Odometry that matches that topic The subscriber to the topic /zed/zed_node/pose calls the poseCallback function when it receives a message of type geometry_msgs/PoseStamped that matches that topic Conclusion The full source code of this tutorial is available on GitHub in the zed_tracking_sub_tutorial sub-package. Along with the node source code, you can find the package.xml and CMakeLists.txt files that complete the tutorial package.