目录

1：nav_msgs/Odometry消息类型

2：发布Odometry消息

3：订阅Odometry消息

1：nav_msgs/Odometry消息类型

查看终端信息数据结构：

rosmsg show nav_msgs/Odometry

Odometry数据结构如下：

Header header string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/TwistWithCovariance twist

其中pose是位置数据，twist是速度数据。 由于还有其他数据结构，这里展开一下，更加清晰：

2：发布Odometry消息

在一个圆圈中行驶的假机器人里程计数据被定义为发布

#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::Publisher和一个tf::TransformBroadcaster可单独使用 ROS 和 tf 发送消息。  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();         current_time = ros::Time::now();    ///以设定的速度更新里程信息   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;    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);    //创建一个TransformStamped消息，通过 tf发布从“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";    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;    odom_broadcaster.sendTransform(odom_trans);    //填充Odometry消息   nav_msgs::Odometry odom;   odom.header.stamp = current_time;   odom.header.frame_id = "odom";    //设置位置   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;    //设置速度   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消息   odom_pub.publish(odom);    last_time = current_time;   r.sleep();  } }

3：订阅Odometry消息

(1) 通过rosbag订阅

rostopic echo /odom

(2) 通过rviz查看 打开rviz

rosrun rviz rviz

Fixed Frame修改为base_link，添加Odometry并将Topic设为/odom(3) 编程打印

#include "ros/ros.h" #include "nav_msgs/Odometry.h" #include "tf/transform_listener.h"  void OdomCallback(const nav_msgs::Odometry::ConstPtr &msg) {    double x, y, z;   double roll, pitch, yaw;   x = msg->pose.pose.position.x;   y = msg->pose.pose.position.y;   z = msg->pose.pose.position.z;   tf::Qaternion quat;                                     //定义一个四元数
    tf::quaternionMsgToTF(msg->pose.pose.orientation, quat); //取出方向存储于四元数
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    ROS_INFO("Odom: %f, %f, %f, %f, %f, %f", x, y, z, roll, pitch, yaw);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle node;
    ros::Subscriber subOdom = node.subscribe("odom", 1000, OdomCallback);
    ros::spin();
    return 0;
}

【完】

下一节会介绍陀螺仪Imu数据的发布和订阅