ROS Navigation-----发布Odometry信息

    本文将给出一个示例说明如何给导航功能包集发布odometry信息， 其中主要包括发布消息nav_msgs/Odometry以及通过tf广播"odom"坐标系到"base_link"坐标系的转换信息。

1 发布Odometry信息

    导航功能包集需要依靠tf 来确定机器人在物理世界中的位置以及传感器数据和静态地图的关系。但受限于tf自身并不能提供任何有关机器人移动速度的信息，所以导航功能包集需要里程源能够同时发布坐标变换和携带速度信息的nav_msgs/Odometry类型消息。

2 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”对应于机器人在odometric坐标系下估计的位置以及一个可选的位姿估计协方差，“twist”对应于机器人子坐标系（通常是移动基座坐标系）下的速度以及一个可选的速度估计协方差。 

3 利用tf发布里程转换

    如在TF配置 Transform Configuration 指导中阐述, "tf"软件库利用transform树来管理机器人上各个坐标系之间的转换。同理里程源也必须发布其坐标系的信息以供统一管理。

4 代码实现

    在本节中，我们将使用伪造的机器人给出可以发布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();
  }
}

以下是代码解读，
1）由于我们需要发布里程坐标系到基座坐标系的转换，以及里程消息nav_msgs/Odometry，所以我们需要包含相应头文件，

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

2）我们需要一个ROS消息发布器发布消息和一个tf transform广播器，因此有如下代码，

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

3）假定机器人初始位置在里程坐标系原点，

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

4）我们预设置几个速度值，让"base_link"坐标系在"odom" 坐标系移动，

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

5）使用我们预先设定的常量速度更新里程信息，当然现实世界中速度是算出来的不是常量，

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;

6）通常在系统中我们尽量使用3D版本的消息，这样方便适当时候2D和3D部件一起工作。 因此，有必要将里程的yaw值转换成四元数来发布。非常幸运的是，tf提供了很容易用yaw创建四元数或者从四元数得到yaw值的函数接口。

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

7）我们将创建一个TransformStamped消息然后通过tf发送它。 我们要在当前时刻发送"odom"坐标系到"base_link"坐标系的转换，因此我们需要设置消息header和子坐标系，并确保"odom"是父坐标系而"base_link"是子坐标系。

geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";

8）用来自里程计的数据填充transform消息，并稍后用TransformBroadcaster发送出去，

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;

9）此外，我们也需要发布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.

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

10）使用里程计数据填充nav_msgs/Odometry消息并发送出去。 我们将该消息子坐标系为"base_link"，因为我们发布的速度信息要给到该坐标系。

    //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;

11）发布里程消息

//publish the message
odom_pub.publish(odom);

总结：基于里程计要发送东西主要包括，

1）"odom"坐标系到"base_link"坐标系里程变换（线位移和角位移的转换）

2）消息nav_msgs/Odometry（线位移和角位移 + 线速度和角速度）