odometry_pubisher_tutorials发布里程数据的教程

主要内容：

提供了一个为导航堆栈发布里程测量信息的示例。 它包括通过ROS发布nav_msgs / Odometry消息，以及通过tf从“odom”坐标框架到“base_link”坐标框架的转换。

1.ROS发布Odometry信息

  导航堆栈使用tf来确定机器人在世界中的位置，并将传感器数据与静态地图相关联。 然而，tf不提供关于机器人的速度的任何信息。 因此，导航堆栈要求任何odometry源通过ROS发布包含速度信息的transform和nav_msgs / Odometry消息。

2.介绍nav_msgs/Odometry消息格式

  nav_msgs/Odometry message存储机器人位置与速度的估测

# 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

该消息中的姿态对应于机器人在测距框架中的估计位置以及该姿态估计的确定性的可选协方差。

 该消息中的twist对应于机器人在子帧（base_laser）中的速度，通常是移动基站的坐标帧，以及该速度估计的确定性的可选协方差。

3. 使用tf发布里程转换（Odometry transform）

如Transform 配置教程中所讨论的，“tf”软件库负责管理与变换树中的机器人相关的坐标系之间的关系。 因此，任何测距源都必须发布其管理的坐标框架的相关信息。

4. 写代码

编写一些用于通过ROS发布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();

  }

}