| Note: This tutorial assumes that you have completed the Transform Configuration tutorial. All the code for this tutorial is available in the odometry_publisher_tutorial package.. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Publishing Odometry Information over ROS
Description: This tutorial provides an example of publishing odometry information for the navigation stack. It covers both publishing the nav_msgs/Odometry message over ROS, and a transform from a "odom" coordinate frame to a "base_link" coordinate frame over tf.Tutorial Level: BEGINNER
Publishing Odometry Information Over ROS
The navigation stack uses tf to determine the robot's location in the world and relate sensor data to a static map. However, tf does not provide any information about the velocity of the robot. Because of this, the navigation stack requires that any odometry source publish both a transform and a nav_msgs/Odometry message over ROS that contains velocity information. This tutorial explains the nav_msgs/Odometry message and provides example code for publishing both the message and transform over ROS and tf respectively.
Contents
The nav_msgs/Odometry Message
The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space:
# This represents an estimate of a position and velocity in free space. # The pose in this message should be specified in the coordinate frame given by header.frame_id. # The twist in this message should be specified in the coordinate frame given by the child_frame_id Header header string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/TwistWithCovariance twist
The pose in this message corresponds to the estimated position of the robot in the odometric frame along with an optional covariance for the certainty of that pose estimate. The twist in this message corresponds to the robot's velocity in the child frame, normally the coordinate frame of the mobile base, along with an optional covariance for the certainty of that velocity estimate.
Using tf to Publish an Odometry transform
As discussed in the Transform Configuration tutorial, the "tf" software library is responsible for managing the relationships between coordinate frames relevant to the robot in a transform tree. Therefore, any odometry source must publish information about the coordinate frame that it manages. The code below assumes a basic knowledge of tf, reading the Transform Configuration tutorial should be sufficient.
Writing the Code
In this section we'll write some example code for publishing a nav_msgs/Odometry message over ROS and a transform using tf for a fake robot that just drives in a circle. We'll show the code in its entirety first, with a piece-by-piece explanation below.
Add the dependancy to your package's manifest.xml
<depend package="tf"/> <depend package="nav_msgs"/>
1 #include <ros/ros.h>
2 #include <tf/transform_broadcaster.h>
3 #include <nav_msgs/Odometry.h>
4
5 int main(int argc, char** argv){
6 ros::init(argc, argv, "odometry_publisher");
7
8 ros::NodeHandle n;
9 ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
10 tf::TransformBroadcaster odom_broadcaster;
11
12 double x = 0.0;
13 double y = 0.0;
14 double th = 0.0;
15
16 double vx = 0.1;
17 double vy = -0.1;
18 double vth = 0.1;
19
20 ros::Time current_time, last_time;
21 current_time = ros::Time::now();
22 last_time = ros::Time::now();
23
24 ros::Rate r(1.0);
25 while(n.ok()){
26
27 ros::spinOnce(); // check for incoming messages
28 current_time = ros::Time::now();
29
30 //compute odometry in a typical way given the velocities of the robot
31 double dt = (current_time - last_time).toSec();
32 double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
33 double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
34 double delta_th = vth * dt;
35
36 x += delta_x;
37 y += delta_y;
38 th += delta_th;
39
40 //since all odometry is 6DOF we'll need a quaternion created from yaw
41 geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
42
43 //first, we'll publish the transform over tf
44 geometry_msgs::TransformStamped odom_trans;
45 odom_trans.header.stamp = current_time;
46 odom_trans.header.frame_id = "odom";
47 odom_trans.child_frame_id = "base_link";
48
49 odom_trans.transform.translation.x = x;
50 odom_trans.transform.translation.y = y;
51 odom_trans.transform.translation.z = 0.0;
52 odom_trans.transform.rotation = odom_quat;
53
54 //send the transform
55 odom_broadcaster.sendTransform(odom_trans);
56
57 //next, we'll publish the odometry message over ROS
58 nav_msgs::Odometry odom;
59 odom.header.stamp = current_time;
60 odom.header.frame_id = "odom";
61
62 //set the position
63 odom.pose.pose.position.x = x;
64 odom.pose.pose.position.y = y;
65 odom.pose.pose.position.z = 0.0;
66 odom.pose.pose.orientation = odom_quat;
67
68 //set the velocity
69 odom.child_frame_id = "base_link";
70 odom.twist.twist.linear.x = vx;
71 odom.twist.twist.linear.y = vy;
72 odom.twist.twist.angular.z = vth;
73
74 //publish the message
75 odom_pub.publish(odom);
76
77 last_time = current_time;
78 r.sleep();
79 }
80 }
Ok, now that you've had a chance to look over everything, let's break down the important parts of the code in detail.
Since we're going to be publishing both a transform from the "odom" coordinate frame to the "base_link" coordinate frame and a nav_msgs/Odometry message, we need to include the relevant header files.
We need to create both a ros::Publisher and a tf::TransformBroadcaster to be able to send messages out using ROS and tf respectively.
We'll assume that the robot starts at the origin of the "odom" coordinate frame initially.
Here we'll set some velocities that will cause the "base_link" frame to move in the "odom" frame at a rate of 0.1m/s in the x direction, -0.1m/s in the y direction, and 0.1rad/s in the th direction. This will more or less cause our fake robot to drive in a circle.
24 ros::Rate r(1.0);
We'll publish odometry information at a rate of 1Hz in this example to make introspection easy, most systems will want to publish odometry at a much higher rate.
30 //compute odometry in a typical way given the velocities of the robot
31 double dt = (current_time - last_time).toSec();
32 double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
33 double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
34 double delta_th = vth * dt;
35
36 x += delta_x;
37 y += delta_y;
38 th += delta_th;
Here we'll update our odometry information based on the constant velocities we set. A real odometry system would, of course, integrate computed velocities instead.
We generally try to use 3D versions of all messages in our system to allow 2D and 3D components to work together when appropriate, and to keep the number of messages we have to create to a minimum. As such, it is necessary to convert our yaw value for odometry into a Quaternion to send over the wire. Fortunately, tf provides functions allowing easy creation of Quaternions from yaw values and easy access to yaw values from Quaternions.
Here we'll create a TransformStamped message that we will send out over tf. We want to publish the transform from the "odom" frame to the "base_link" frame at current_time. Therefore, we'll set the header of the message and the child_frame_id accordingly, making sure to use "odom" as the parent coordinate frame and "base_link" as the child coordinate frame.
Here we fill in the transform message from our odometry data, and then send the transform using our TransformBroadcaster.
We also need to publish a nav_msgs/Odometry message so that the navigation stack can get velocity information from it. We'll set the header of the message to the current_time and the "odom" coordinate frame.
62 //set the position
63 odom.pose.pose.position.x = x;
64 odom.pose.pose.position.y = y;
65 odom.pose.pose.position.z = 0.0;
66 odom.pose.pose.orientation = odom_quat;
67
68 //set the velocity
69 odom.child_frame_id = "base_link";
70 odom.twist.twist.linear.x = vx;
71 odom.twist.twist.linear.y = vy;
72 odom.twist.twist.angular.z = vth;
This populates the message with odometry data and sends it out over the wire. We'll set the child_frame_id of the message to be the "base_link" frame since that's the coordinate frame we're sending our velocity information in.
