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 robotdouble 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 yawgeometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//first, we'll publish the transform over tfgeometry_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 transformodom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROSnav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the positionodom.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 velocityodom.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 messageodom_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 robotdouble 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 yawgeometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布，辛运的是，ROS为我们提供了偏航角与四元数相互转换的功能。
//first, we'll publish the transform over tfgeometry_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 ROSnav_msgs::Odometry odom;
odom.header.stamp = current_time;
别忘了，我们还要发布nav_msgs/Odometry消息，让导航包获取机器人的速度。创建消息变量，然后填充时间戳。
//set the positionodom.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 velocityodom.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
多年以后
古月老师你好，我目前在机器人上装了麦克风用于声源定位，但是想结合导航的避障功能进行声源追踪，我该怎么转换麦克风定的位置信息转换传给movebase让它实现追踪呢？我麦克风定位的信息是属于上面的odom，base_link中的哪一个呢？
古月
@多年以后 应该属于odom
nick
古老师，
我知道代码中delta_x，delta_y的计算公式是怎么得来的？
古月
@nick 速度在x、y轴上的分解
fason_zh
你好，关于odom到base_link的tf变换和topic消息传递不太明白，我在用amcl定位的时候，发现只要在tf那边给了正确的数据，topic msg这边即使全部赋值0也不影响amcl运行，请问这是为什么呢？
古月
@fason_zh amcl的输入是激光雷达的信息，并不是里程计的数据，输出是基于激光的定位估计
李志明
古老师，我是ros的一个初学者。在学习的过程中发现像odom.pose.pose.position这类的东西很疑惑。为什么要加那么多小数点。想知道是什么结构。还请指教。谢谢古老师
古月
@李志明 这个就是编程语言的数据结构，只不过包含的层次多了一些，wiki上可以查到odom的具体数据结构定义
zero
您好，我现在想用gps+imu来替代里程计，请问需要读取什么数据，替换掉您的代码的哪些地方
古月
@zero 计算位置信息，替换上边的x、y、th
zero
@古月 古老师，我现在将gps+imu输出的位置经过坐标转换作为odom的Pose输入，然后线速度与角速度也替换掉了，关于发布的/odom主题的四元数和协方差我还是不太能理解，在实际试验中，这个四元数和协方差应该如何解决？谢谢古老师！
古月
@zero 四元数和协方差理论方面的内容请参考相关的机器人书籍或网上资料
terminal
老师，您好！我想问一下，我采用的是增量式编码器，通过串口需要从编码器中读取什么数据？读取到的数据是填到您代码中的什么地方？
古月
@terminal 具体要结合你是用传感器的数据内容，主要是当前的速度填到代码中，积分得到位置
terminal
@古月 老师，那是把编码器的数据处理成速度，然后替换程序中的这部分吗？
double vx = 0.1;
double vy = -0.1;
double vth = 0.1;
把这个速度换成从编码器获得的速度？
古月
@terminal 是的
pety
你好，古月大神，我按照你的程序发布了里程计信息，在RVIZ中模拟时，也发现机器人会动，发布消息的时候，机器人模型会跳到实际的位置，但是消息发完后，机器人模型又回到原来的位置，在RVIZ中观察，机器人一跳一跳的，不知道哪的原因，谢谢
古月
@pety 检查是不是有其他节点在发布位置的tf变换
pety
@古月 解决了，应该是模拟器发布的的ｔｆ变换。现在发现另外一个问题，我启动的机器人模型，然后启动了map_server 和move_base,然后利用程序发布里程计信息（我把里程计的值设为０），然后在ｒｖｉｚ中设置目标点，ｒｖｉｚ中有提示，说目标点设置成功，但是监控cmd_vel,没有任何的数据发送，不知道怎么回事。 我主要想利用伪造的里程计信息，测试一下move_base,监控发送的cmd_vel,然后可以直接移植到小车上。古月大神，是否可以讲一下，谢谢。
现在教程看了一遍，在模拟环境下测试了一下，但是还是感觉和实际的机器人硬件有点脱轨。
古月
@pety 里程计模拟的话最好有变化，可以走一个圆出来，可以参考《ROS by example》
fish9210
古月大，您好！
最近正在学习您的《ROS机器人开发实践》，受益匪浅！
在学习第九章SLAM时，我自己在尝试搭建一个ROS底盘。
采用的是ROSArduinoBridge包，arduino作为底层硬件。现在已经实现了从ROS向Arduino发送速度消息，轮子也能转起来，运动方向也是正确的。（还未组装底盘，只是观察轮子运动的方向）。
目前，遇到一个问题。当我发送，linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}，让人意外的是，
从rviz中观察到（以odom作为固定框架），base_link的轨迹，居然是在画圆。我原本期望轨迹应该是走直线。
造成这个问题的原因，是不是因为，发布里程计的代码计算方式的问题？下面这是rosarduinobridge包中的base_controller.py 的代码，请您看下。究竟如何才能显示直线的轨迹呢？
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame, // 子坐标系
"odom" // 父坐标系
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
谢谢您的指点！
古月
@fish9210 程序还是自己调试一下，走直线的时候不会有角速度，调试看下变量的值
Ziping
你好，我想问一下就是我现在可以用编码器读取单位时间内电机转过的圈数，然后要怎么转换成速度（我问的是一个单位问题，就是怎么转换成odom用的数据，因为我后续要用robot_pose_ekf对编码器和imu进行卡尔曼融合），所以这个单位怎么去统一？ 🙂
古月
@Ziping ROS中的线速度单位是m/s，角速度单位是rad/s
Ziping
@古月 好的，那假如我现在要slam建图，是不是odom需要和base_link同步，上文中程序base_link替换为map？因为我的odom是相对map移动的，需要这样修改吗？
古月
@Ziping 不用的，坐标系之间的变换是slam功能包完成的，自己只要发布odom和base_link之间的变换就可以了
Ziping
你好，我想问一下就是我现在可以用编码器读取单位时间内电机转过的圈数，然后要怎么转换成速度（我问的是一个单位问题，就是怎么转换成odom用的数据，因为我后续要用robot_pose_ekf对编码器和imu进行卡尔曼融合），所以这个单位怎么去统一？
Billy
你好，古月大师。请问一下，如果自己搭建的小车无法输出运动速度和方向，也就是没有里程计信息，想通过激光雷达(Hokuyo)的扫描数据在地图(地图用hector_mapping得到)中定位，是否可行？顺便问一句，使用里程计信息进行导航的纠偏原理是什么？
古月
@Billy 可以的， 使用hector包就可以实现激光定位。使用里程计会有累积误差，可以用卡尔曼滤波减少误差，同时配合其他传感器纠正定位。
Billy
@古月 多谢指教！还有个疑问，hector实现绘制地图与定位，那如何在绘制的地图中进行导航与纠偏呢？还能使用move_base包吗？我看move_base包订阅了/odom主题，但我没有里程计信息，怎么实现呢？
古月
@Billy hector和gmapping建立的地图都是一样的，都可以用来导航，导航必须要有里程计话题的，里程计有很多种，如果你都没有的话，找下ROS里有没有基于激光雷达实现里程计话题的功能包
曾杰
你好 古月 最近在用gazebo做强化学习，看的东西有些糊涂了。现在我在我的程序中提取小车的实时坐标，我想就用里程计的数据来表示坐标，可是看到了tf，似乎需要将里程计信息转换成map坐标？可是gazebo里面没有map啊，有些迷茫，希望能给点建议
古月
@曾杰 里程计可以输出坐标的，是odom那个话题
Aaron
您好，想问下这个如何得到真实的odom，是要时时更新vx vy vth么？我用gmapping，需要odom信息，才能发布速度，那我odom又要订阅速度。。。。有点懵。。。。
古月
@Aaron 你好，速度是积分算出来的，也就是单位时间内的编码器旋转圈数，然后填入odom
Aaron
@古月 我原先这么想的，但是我不太知道是不是需要再写一个节点去读编码器的值运算后发布出速度信息，之后odom再去订阅获得信息
古月
@Aaron 可以不用，在发布odom的节点里做就行了
Aaron
@古月 因为我是用串口通信。那我就先用串口读车的编码器的数据，进行积分，代替vx等，算出速度串口再发给地盘。这样理解对么？我用编码器算出的速度和我最后用slam算出的速度的差异在哪里。
古月
@Aaron 可以的，很多机器人都是这样做的，通过串口直接读取到速度。这个地方你可能理解的不是很清楚，SLAM并不计算速度，相反的，SLAM需要机器人提供速度信息，也就是odom消息。这篇博客是在odom中积分计算速度的，和你在底盘控制器上计算速度原理一样，只是位置不同而已。
Liu
不知道为何重复送出了.. 造成版面混乱实在不好意思
古月
@Liu 没关系，网站问题，我已经删除了多余的留言
Liu
古月大 您好!
我最近刚接触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 值 已经困扰很久但是一直找不到方法
希望古月大能够指点一番
古月
@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
您好，我是最近接触tf与导航系统的。我不太清楚这里的协方差是用来表示什么的？是谁与谁的协方差？希望能回答，谢谢。
古月
@acyloin 你好，这里协方差的概念目前我也不是很理解，涉及到位置估计方面的算法，我还没有研究过这个方面。
蒋卓
@古月 您好，我想问下tf的根坐标系是怎么建立的，也就是第一个坐标系是怎么确定的？
古月
@蒋卓 源头的parent_link就是第一个坐标系
alert(/1/)
@acyloin alert(/1/) :razz:好的