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 messagesPosition tolerance
andAngle tolerance
: set both to0
to be sure to visualize all positional dataKeep
: Set to the number of messages to visualize at a timeShape
: You can visualize odometry information as an arrow pointing in the direction of theX
axis, or as a threeAxes
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 of 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 to 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 Mini:
$ roslaunch zed_wrapper zedm.launch
ZED 2:
$ roslaunch zed_wrapper zed2.launch
ZED 2i:
$ 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 is calculated as the sum of the movements relative to the previous position. Only pure visual odometry is used
pose
: The position calculated relative to theworld 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 an SVO file, you will receive the following stream of messages confirming that you 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 of the source code above.
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/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.
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 theodomCallback
function when it receives a message of typenav_msgs/Odometry
that matches that topic - The subscriber to the topic
/zed/zed_node/pose
calls theposeCallback
function when it receives a message of typegeometry_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.