【cartographer_ros】四： 发布和订阅里程计odom信息

上一节介绍了激光雷达Scan传感数据的订阅和发布。

 

本节会介绍里程计Odom数据的发布和订阅。里程计在cartographer中主要用于前端位置预估和后端优化。

官方文档：
http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

---

目录

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::Quaternion 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数据的发布和订阅