ROS探索总结-21.如何发布里程计消息

如何发布里程计消息

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