（六）ROS发布里程计（Odometry）消息并在rviz中显示

由于tf只能表示机器人相对与全局地图 world的位置关系，而里程计消息不但能表示位置还能够表示向量信息。

里程计消息nav_msgs/Odometry 结构如下

Header header

string child_frame_id

geometry_msgs/PoseWithCovariance pose

geometry_msgs/TwistWithCovariance twist

ROS中可以发布里程计消息，并在rviz中显示。

本文参考wiki上的教程实现在rviz中显示里程计的功能

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

步骤

（1）创建里程计发布节点

（2）在rviz中显示

1.创建里程计发布节点

（1）编译代码

此节点实现的功能是绕原点转圈

代码如下

#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");

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();
}
}

（2）启动节点

编译的包名为learning_tf 节点为编译好的可执行程序odometry

rosrun learning_tf odometry

2.在rviz中显示

rosrun rviz rviz

在左边Display 配置属性，fixed frame选择为odom，add 选项加入Odemetry,在topic下拉框中选择/odem

3实验结果

是不是发现和http://blog.csdn.net/ktigerhero3/article/details/64906315

中的示例相似，示例中机器人运动发布的就是里程计数据。