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 支付宝扫描二维码

为本文作者打个赏

5条评论

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:好的

发表评论取消回复

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探索总结（三十八）——有限状态机smach (2)

ROS探索总结（三十七）——有限状态机smach (1)

ROS探索总结（三十六）——Matlab中的ROS可视化应用

ROS探索总结（三十五）——Matlab中的ROS

2017 ROS Summer School见闻

2017 ROS Summer School见闻

ROS探索总结（三十）——3D地图建模

ROS探索总结（九）——操作杆控制

ROS探索总结（二十）——发布导航需要的传感器信息

ROS2探索总结（五）——ROS1与ROS2之间的桥梁

1、nav_msgs/Odometry消息格式

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

3、代码例程

发表评论 取消回复

发表评论取消回复