ROS探索总结（二十一）——如何发布里程计消息

ROS使用tf来决定机器人的位置和静态地图中的传感器数据，但是tf中没有机器人的速度信息，所以导航功能包要求机器人能够通过里程计信息源发布包含速度信息的里程计nav_msgs/Odometry 消息。本篇将介绍nav_msgs/Odometry消息，并且通过代码实现消息的发布，以及tf树的变换。

1、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
```

pose参数包含机器人在里程计参考系下的位置估算值，同时带有可选的估算协方差。twist参数包含机器人在子参考系（一般是机器人基础参考系）下的速度，同时带有可选的速度估算协方差。

2、使用tf发布里程计变换信息

参见 Transform Configuration教程。

3、代码例程

这里使用一个简单的例程，实现 nav_msgs/Odometry消息的发布和tf变换，通过伪造的数据，实现机器人圆周运动。

首先需要将一些以来添加到manifest.xml中：

```
<depend package="tf"/>
```
```
<depend package="nav_msgs"/>
```

实现代码如下：

```
#include <ros/ros.h>
```
```
#include <tf/transform_broadcaster.h>
```
```
#include <nav_msgs/Odometry.h>
```
```
 
```
```
int main(int argc, char** argv){
```

  ros::init(argc, argv, "odometry_publisher");

```
 
```
```
  ros::NodeHandle n;
```

  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);

  tf::TransformBroadcaster odom_broadcaster;

```
 
```
```
  double x = 0.0;
```
```
  double y = 0.0;
```
```
  double th = 0.0;
```
```
 
```
```
  double vx = 0.1;
```
```
  double vy = -0.1;
```
```
  double vth = 0.1;
```
```
 
```
```
  ros::Time current_time, last_time;
```
```
  current_time = ros::Time::now();
```
```
  last_time = ros::Time::now();
```
```
 
```
```
  ros::Rate r(1.0);
```
```
  while(n.ok()){
```
```
 
```

    ros::spinOnce();               // check for incoming messages

```
    current_time = ros::Time::now();
```
```
 
```

    //compute odometry in a typical way given the velocities of the robot

    double dt = (current_time - last_time).toSec();

    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;

    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;

```
    double delta_th = vth * dt;
```
```
 
```
```
    x += delta_x;
```
```
    y += delta_y;
```
```
    th += delta_th;
```
```
 
```

    //since all odometry is 6DOF we'll need a quaternion created from yaw

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

```
 
```

    //first, we'll publish the transform over tf

    geometry_msgs::TransformStamped odom_trans;

    odom_trans.header.stamp = current_time;

    odom_trans.header.frame_id = "odom";

    odom_trans.child_frame_id = "base_link";

```
 
```

    odom_trans.transform.translation.x = x;

    odom_trans.transform.translation.y = y;

    odom_trans.transform.translation.z = 0.0;

    odom_trans.transform.rotation = odom_quat;

```
 
```
```
    //send the transform
```

    odom_broadcaster.sendTransform(odom_trans);

```
 
```

    //next, we'll publish the odometry message over ROS

```
    nav_msgs::Odometry odom;
```
```
    odom.header.stamp = current_time;
```
```
    odom.header.frame_id = "odom";
```
```
 
```
```
    //set the position
```
```
    odom.pose.pose.position.x = x;
```
```
    odom.pose.pose.position.y = y;
```
```
    odom.pose.pose.position.z = 0.0;
```

    odom.pose.pose.orientation = odom_quat;

```
 
```
```
    //set the velocity
```
```
    odom.child_frame_id = "base_link";
```
```
    odom.twist.twist.linear.x = vx;
```
```
    odom.twist.twist.linear.y = vy;
```
```
    odom.twist.twist.angular.z = vth;
```
```
 
```
```
    //publish the message
```
```
    odom_pub.publish(odom);
```
```
 
```
```
    last_time = current_time;
```
```
    r.sleep();
```
```
  }
```
```
}
```

下面来剖析代码进行分析：

```
#include <tf/transform_broadcaster.h>
```
```
#include <nav_msgs/Odometry.h>
```

我们需要实现“odom”参考系到“base_link”参考系的变换，以及nav_msgs/Odometry消息的发布，所以首先需要包含相关的头文件。

  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);

  tf::TransformBroadcaster odom_broadcaster;

定义一个消息发布者来发布“odom”消息，在定义一个tf广播，来发布tf变换信息。

```
  double x = 0.0;
```
```
  double y = 0.0;
```
```
  double th = 0.0;
```

默认机器人的起始位置是odom参考系下的0点。

```
  double vx = 0.1;
```
```
  double vy = -0.1;
```
```
  double vth = 0.1;
```

我们设置机器人的默认前进速度，让机器人的base_link参考系在odom参考系下以x轴方向0.1m/s，Y轴速度-0.1m/s，角速度0.1rad/s的状态移动，这种状态下，可以让机器人保持圆周运动。

```
  ros::Rate r(1.0);
```

使用1Hz的频率发布odom消息，当然，在实际系统中，往往需要更快的速度进行发布。

    //compute odometry in a typical way given the velocities of the robot

    double dt = (current_time - last_time).toSec();

    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;

    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;

```
    double delta_th = vth * dt;
```
```
 
```
```
    x += delta_x;
```
```
    y += delta_y;
```
```
    th += delta_th;
```

使用我们设置的速度信息，来计算并更新里程计的信息，包括单位时间内机器人在x轴、y轴的坐标变化和角度的变化。在实际系统中，需要更具里程计的实际信息进行更新。

   //since all odometry is 6DOF we'll need a quaternion created from yaw

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布，辛运的是，ROS为我们提供了偏航角与四元数相互转换的功能。

    //first, we'll publish the transform over tf

    geometry_msgs::TransformStamped odom_trans;

    odom_trans.header.stamp = current_time;

    odom_trans.header.frame_id = "odom";

    odom_trans.child_frame_id = "base_link";

创建一个tf发布需要使用的TransformStamped类型消息，然后根据消息结构填充当前的时间戳、参考系id、子参考系id，注意两个参考系的id必须要是“odom”和“base_link”。

    odom_trans.transform.translation.x = x;

    odom_trans.transform.translation.y = y;

    odom_trans.transform.translation.z = 0.0;

    odom_trans.transform.rotation = odom_quat;

填充里程计信息，然后发布tf变换的消息。

    //next, we'll publish the odometry message over ROS

```
    nav_msgs::Odometry odom;
```
```
    odom.header.stamp = current_time;
```

别忘了，我们还要发布nav_msgs/Odometry消息，让导航包获取机器人的速度。创建消息变量，然后填充时间戳。

```
    //set the position
```
```
    odom.pose.pose.position.x = x;
```
```
    odom.pose.pose.position.y = y;
```
```
    odom.pose.pose.position.z = 0.0;
```

    odom.pose.pose.orientation = odom_quat;

```
 
```
```
    //set the velocity
```
```
    odom.child_frame_id = "base_link";
```
```
    odom.twist.twist.linear.x = vx;
```
```
    odom.twist.twist.linear.y = vy;
```
```
    odom.twist.twist.angular.z = vth;
```

填充机器人的位置、速度，然后发布消息。注意，我们发布的是机器人本体的信息，所以参考系需要填"base_link"。

参考链接：http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

原创文章，转载请注明： 转载自古月居

本文链接地址: ROS探索总结（二十一）——如何发布里程计消息

用微信 OR 支付宝扫描二维码

为本文作者打个赏

26条评论

Ziping 2018-05-06 23:04:20 回复

你好，我想问一下就是我现在可以用编码器读取单位时间内电机转过的圈数，然后要怎么转换成速度（我问的是一个单位问题，就是怎么转换成odom用的数据，因为我后续要用robot_pose_ekf对编码器和imu进行卡尔曼融合），所以这个单位怎么去统一？ 🙂
- 古月 2018-05-08 09:29:49 回复
  
  @Ziping ROS中的线速度单位是m/s，角速度单位是rad/s
  - Ziping 2018-05-10 13:28:24 回复
    
    @古月好的，那假如我现在要slam建图，是不是odom需要和base_link同步，上文中程序base_link替换为map？因为我的odom是相对map移动的，需要这样修改吗？
    - 古月 2018-05-15 20:12:39 回复
      
      @Ziping 不用的，坐标系之间的变换是slam功能包完成的，自己只要发布odom和base_link之间的变换就可以了
Ziping 2018-05-06 23:03:33 回复

你好，我想问一下就是我现在可以用编码器读取单位时间内电机转过的圈数，然后要怎么转换成速度（我问的是一个单位问题，就是怎么转换成odom用的数据，因为我后续要用robot_pose_ekf对编码器和imu进行卡尔曼融合），所以这个单位怎么去统一？
Billy 2018-04-13 13:23:42 回复

你好，古月大师。请问一下，如果自己搭建的小车无法输出运动速度和方向，也就是没有里程计信息，想通过激光雷达(Hokuyo)的扫描数据在地图(地图用hector_mapping得到)中定位，是否可行？顺便问一句，使用里程计信息进行导航的纠偏原理是什么？
- 古月 2018-04-16 10:35:33 回复
  
  @Billy 可以的，使用hector包就可以实现激光定位。使用里程计会有累积误差，可以用卡尔曼滤波减少误差，同时配合其他传感器纠正定位。
  - Billy 2018-04-16 11:31:12 回复
    
    @古月多谢指教！还有个疑问，hector实现绘制地图与定位，那如何在绘制的地图中进行导航与纠偏呢？还能使用move_base包吗？我看move_base包订阅了/odom主题，但我没有里程计信息，怎么实现呢？
    - 古月 2018-04-16 18:07:43 回复
      
      @Billy hector和gmapping建立的地图都是一样的，都可以用来导航，导航必须要有里程计话题的，里程计有很多种，如果你都没有的话，找下ROS里有没有基于激光雷达实现里程计话题的功能包
曾杰 2018-04-09 20:47:00 回复

你好古月最近在用gazebo做强化学习，看的东西有些糊涂了。现在我在我的程序中提取小车的实时坐标，我想就用里程计的数据来表示坐标，可是看到了tf，似乎需要将里程计信息转换成map坐标？可是gazebo里面没有map啊，有些迷茫，希望能给点建议
- 古月 2018-04-10 14:29:37 回复
  
  @曾杰里程计可以输出坐标的，是odom那个话题
Aaron 2018-01-18 22:20:13 回复

您好，想问下这个如何得到真实的odom，是要时时更新vx vy vth么？我用gmapping，需要odom信息，才能发布速度，那我odom又要订阅速度。。。。有点懵。。。。
- 古月 2018-01-19 20:43:12 回复
  
  @Aaron 你好，速度是积分算出来的，也就是单位时间内的编码器旋转圈数，然后填入odom
  - Aaron 2018-02-07 10:56:58 回复
    
    @古月我原先这么想的，但是我不太知道是不是需要再写一个节点去读编码器的值运算后发布出速度信息，之后odom再去订阅获得信息
    - 古月 2018-02-07 11:34:38 回复
      
      @Aaron 可以不用，在发布odom的节点里做就行了
      - Aaron 2018-02-07 16:38:38 回复
        
        @古月因为我是用串口通信。那我就先用串口读车的编码器的数据，进行积分，代替vx等，算出速度串口再发给地盘。这样理解对么？我用编码器算出的速度和我最后用slam算出的速度的差异在哪里。
        
        古月 2018-02-08 11:39:01 回复
        
        @Aaron 可以的，很多机器人都是这样做的，通过串口直接读取到速度。这个地方你可能理解的不是很清楚，SLAM并不计算速度，相反的，SLAM需要机器人提供速度信息，也就是odom消息。这篇博客是在odom中积分计算速度的，和你在底盘控制器上计算速度原理一样，只是位置不同而已。
Liu 2018-01-03 00:10:10 回复

不知道为何重复送出了.. 造成版面混乱实在不好意思
- 古月 2018-01-09 09:14:46 回复
  
  @Liu 没关系，网站问题，我已经删除了多余的留言
Liu 2018-01-03 00:07:55 回复

古月大您好!
我最近刚接触ROS，使用的是Turtlebot机器人
他的/odom child_frame_id是base_footprint
我是否能透过这种发布方式改变/odom Pose的位置呢?

我有写个Script如下想透过这种方式改变机器人的Pose 好像不行

odom = Odometry()
odom.pose.pose.position.x = 0
odom.pose.pose.position.y = 0
odom.pose.pose.position.z = 0

publish(odom)

只有Script在执行的时候可以在 rostopic echo /odom看的见值得变化
可是停止执行后 rostopic echo /odom 显示的值又是原本的毫无改变

想要写个code 可以直接改变Pose 值已经困扰很久但是一直找不到方法
希望古月大能够指点一番
- 古月 2018-01-09 09:34:32 回复
  
  @Liu 您好，这种方法是可以修改机器人的位置，但是除了发布odom消息外，还需要修改odom的tf变换，你在代码中的实现流程应该是这样的：
  //first, we'll publish the transform over tf
  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = current_time;
  odom_trans.header.frame_id = "odom";
  odom_trans.child_frame_id = "base_link";
  
  odom_trans.transform.translation.x = x;
  odom_trans.transform.translation.y = y;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = odom_quat;
  
  //send the transform
  odom_broadcaster.sendTransform(odom_trans);
  
  //next, we'll publish the odometry message over ROS
  nav_msgs::Odometry odom;
  odom.header.stamp = current_time;
  odom.header.frame_id = "odom";
  
  //set the position
  odom.pose.pose.position.x = x;
  odom.pose.pose.position.y = y;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;
  
  //set the velocity
  odom.child_frame_id = "base_link";
  odom.twist.twist.linear.x = vx;
  odom.twist.twist.linear.y = vy;
  odom.twist.twist.angular.z = vth;
  
  //publish the message
  odom_pub.publish(odom);
  另外，可以参考《ROS by example》这本书中的Turtlebot控制，有配套的代码可以参考
acyloin 2016-06-29 18:32:22 回复

您好，我是最近接触tf与导航系统的。我不太清楚这里的协方差是用来表示什么的？是谁与谁的协方差？希望能回答，谢谢。
- 古月 2016-06-30 17:30:20 回复
  
  @acyloin 你好，这里协方差的概念目前我也不是很理解，涉及到位置估计方面的算法，我还没有研究过这个方面。
  - 蒋卓 2016-11-13 20:59:46 回复
    
    @古月您好，我想问下tf的根坐标系是怎么建立的，也就是第一个坐标系是怎么确定的？
    - 古月 2016-11-14 09:24:18 回复
      
      @蒋卓源头的parent_link就是第一个坐标系
- alert(/1/) 2017-06-22 17:32:08 回复
  
  @acyloin alert(/1/) :razz:好的

古月居

ROS探索总结（二十一）——如何发布里程计消息

1、nav_msgs/Odometry消息格式

2、使用tf发布里程计变换信息

3、代码例程

评论

发表评论取消回复

ROS探索总结（五十三）—— ROS与VREP的集成

文末惊喜 ||《ROS机器人开发实践》上线开售

《ROS机器人开发实践》| 推荐序 · 林天麟博士

《ROS机器人开发实践》| 推荐序 · 刘锦涛博士

《ROS机器人开发实践》| 推荐序 · 张新宇博士

ROS探索总结（十八）——重读tf

ROS探索总结（三十一）——ros_control

ROS探索总结（二十一）——如何发布里程计消息

ROS探索总结（二十六）——MoveIt编程

ROS2探索总结（三）——小试牛刀

1、nav_msgs/Odometry消息格式

2、使用tf发布里程计变换信息

3、代码例程

发表评论 取消回复

发表评论取消回复