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.

通过ROS发布里程计信息

通过ROS发布里程计信息

导航包使用tf来确定机器人在地图中的位置和建立传感器数据与静态地图的联系。然而TF不能提供任何关于机器人速度的信息 ，所以导航包要求任何的里程计源能通过ROS 发布TF变换和 包含速度信息的nav_msgs/Odometry类型的消息这个教程解释了nav_msgs/Odometry 消息和提供通过ROS发布消息和变换样例代码。

nav_msgs/Odometry 消息

nav_msgs/Odometry 消息保存了机器人空间里评估的位置和速度信息。

# 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.

Error: No code_block found Since we're going to be publishing both a transfrom 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.

Error: No code_block found We need to create both a ros::Publisher and a tf::TransformBroadcaster to be able to send messages out using ROS and tf respectively.

Error: No code_block found We'll assume that the robot starts at the origin of the "odom" coordinate frame initially.

Error: No code_block found 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.

Error: No code_block found 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.

Error: No code_block found 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.

Error: No code_block found 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.

Error: No code_block found 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.

Error: No code_block found Here we fill in the transform message from our odometry data, and then send the transform using our TransformBroadcaster.

Error: No code_block found 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.

Error: No code_block found 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.