在ROS上發佈Odometry信息
描述： 這一教程提供了一個爲Navigation Stack發佈Odometry消息的例子，不僅包含了如何發佈nav_msgs/Odometry消息，還包括一個”odom”座標幀到“base_link”座標幀的變換。
1. 在ROS上發佈Odometry信息
Navigation Stack使用tf來判斷機器人的位置以及對傳感器數據進行變換，但是tf並不包含任何關於機器人速度的信息。因此，Navigation Stack需要一個Odometry source來發佈一個變換關係以及一個包含速度信息的nav_msgs/Odometry消息。本文將解釋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字段對應於Odometry幀中對機器人的估計位置，該字段包含了一個可選的位置估計的協方差字段。twist字段則對應於child 幀中機器人的速度，一般是移動基座（mobile base）的座標幀，該字段包含了一個可選的速度估計的協方差字段。
3. 使用tf發佈Odometry變換
在變換配置教程,我們介紹了tf是通過變換樹來管理座標幀轉換之間的關係的。因此，任何Odometry source都必須發佈其管理的座標幀相關的信息，閱讀下面的內容前請先了解tf的用法。
4. 編碼
這一部分我們將爲一個虛擬的轉圈機器人編寫一個發佈nav_msgs/Odometry消息以及變換關係的例子。
在你的package.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消息，所以include這兩個頭文件。
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;創建發佈器以及tf::TransformBroadcaster來發布消息和tf。
double x = 0.0;
double y = 0.0;
double th = 0.0;假設機器人從odom座標幀的起始位置開始。
double vx = 0.1;
double vy = -0.1;
double vth = 0.1;設置一些速度，讓base_link幀在odom幀中以x方向0.1m/s,y方向-0.1m/s以及th方向0.1rad/s進行運動，這或多或少會讓我們的虛擬機器人轉圈。
ros::Rate r(1.0);簡單起見，這裏以1Hz的頻率發佈Odometry信息，大多數系統會希望以更高的頻率發佈odometry。
//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;這裏我們基於設置的固定速度來更新Odometry信息，當然，真實的Odometry系統會結合計算的真實速度來更新。
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);一般我們會在系統中使用所有信息的3D版本來兼容2D和3D的信息，並儘量減少所需要創建的消息數量。因此，我們需要將我們yaw數值轉換爲一個四元組（Quaternion）。幸運的是，tf提供了從yaw創建Quaternion的接口，並能從Quaternion中方便地操作yaw數據。
//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消息，我們希望在current_time從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;
//send the transform
odom_broadcaster.sendTransform(odom_trans);這裏我們將根據odometry數據填充transform消息，然後通過TransformBroadcaster發佈該變換。
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";我們還需要發佈nav_msgs/Odometry消息以讓navigation stack能接收速度信息，這裏將消息頭設爲current_time以及“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;這裏在消息中填充了odometry數據，這裏將消息的child_frame_id設爲“base_link”，因爲這是我們要將速度信息發往的座標幀。