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

人工智能110

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

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

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

目录

查看终端上的消息数据结构:

[En]

View the message data structure on the terminal:

rosmsg show nav_msgs/Odometry

Odometry消息类型数据结构如下:

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

其中pose是位置数据,twist是速度数据。
由于还有其他数据结构,请在此处展开以使其更清楚一些:

[En]

Because there are other data structures, expand here to make it a little clearer:

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

2:发布Odometry消息

定义假机器人在圆圈中行驶的里程计数据以供发布。

[En]

Defines the odometer data of a fake robot driving in a circle for publishing.

#include <ros ros.h>
#include <tf transform_broadcaster.h>
#include <nav_msgs odometry.h>

int main(int argc, char** argv){
&#xA0; ros::init(argc, argv, "odometry_publisher");

&#xA0; //&#x521B;&#x5EFA;&#x4E00;&#x4E2A;ros::Publisher&#x548C;&#x4E00;&#x4E2A;tf::TransformBroadcaster&#x4EE5;&#x4FBF;&#x80FD;&#x591F;&#x5206;&#x522B;&#x4F7F;&#x7528; ROS &#x548C; tf &#x53D1;&#x9001;&#x6D88;&#x606F;&#x3002;
&#xA0; ros::NodeHandle n;
&#xA0; ros::Publisher odom_pub = n.advertise<nav_msgs::odometry>("odom", 50);
&#xA0; tf::TransformBroadcaster odom_broadcaster;

&#xA0; double x = 0.0;
&#xA0; double y = 0.0;
&#xA0; double th = 0.0;

&#xA0; double vx = 0.1;
&#xA0; double vy = -0.1;
&#xA0; double vth = 0.1;

&#xA0; ros::Time current_time, last_time;
&#xA0; current_time = ros::Time::now();
&#xA0; last_time = ros::Time::now();

&#xA0; ros::Rate r(1.0);
&#xA0; while(n.ok()){

&#xA0; &#xA0; ros::spinOnce(); &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0;
&#xA0; &#xA0; current_time = ros::Time::now();

&#xA0; &#xA0; //&#x6839;&#x636E;&#x8BBE;&#x7F6E;&#x7684;&#x901F;&#x5EA6;&#x66F4;&#x65B0;&#x91CC;&#x7A0B;&#x4FE1;&#x606F;
&#xA0; &#xA0; double dt = (current_time - last_time).toSec();
&#xA0; &#xA0; double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
&#xA0; &#xA0; double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
&#xA0; &#xA0; double delta_th = vth * dt;

&#xA0; &#xA0; x += delta_x;
&#xA0; &#xA0; y += delta_y;
&#xA0; &#xA0; th += delta_th;

&#xA0; &#xA0; geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

&#xA0; &#xA0; //&#x521B;&#x5EFA;&#x4E00;&#x4E2A;TransformStamped&#x6D88;&#x606F;&#xFF0C;&#x901A;&#x8FC7; tf&#x53D1;&#x5E03;&#x4ECE;&#x201C;odom&#x201D;&#x5230;&#x201C;base_link&#x201D;&#x7684;&#x8F6C;&#x6362;
&#xA0; &#xA0; geometry_msgs::TransformStamped odom_trans;
&#xA0; &#xA0; odom_trans.header.stamp = current_time;
&#xA0; &#xA0; odom_trans.header.frame_id = "odom";
&#xA0; &#xA0; odom_trans.child_frame_id = "base_link";

&#xA0; &#xA0; odom_trans.transform.translation.x = x;
&#xA0; &#xA0; odom_trans.transform.translation.y = y;
&#xA0; &#xA0; odom_trans.transform.translation.z = 0.0;
&#xA0; &#xA0; odom_trans.transform.rotation = odom_quat;

&#xA0; &#xA0; odom_broadcaster.sendTransform(odom_trans);

&#xA0; &#xA0; //&#x586B;&#x5145;Odometry&#x6D88;&#x606F;
&#xA0; &#xA0; nav_msgs::Odometry odom;
&#xA0; &#xA0; odom.header.stamp = current_time;
&#xA0; &#xA0; odom.header.frame_id = "odom";

&#xA0; &#xA0; //&#x8BBE;&#x7F6E;&#x4F4D;&#x7F6E;
&#xA0; &#xA0; odom.pose.pose.position.x = x;
&#xA0; &#xA0; odom.pose.pose.position.y = y;
&#xA0; &#xA0; odom.pose.pose.position.z = 0.0;
&#xA0; &#xA0; odom.pose.pose.orientation = odom_quat;

&#xA0; &#xA0; //&#x8BBE;&#x7F6E;&#x901F;&#x5EA6;
&#xA0; &#xA0; odom.child_frame_id = "base_link";
&#xA0; &#xA0; odom.twist.twist.linear.x = vx;
&#xA0; &#xA0; odom.twist.twist.linear.y = vy;
&#xA0; &#xA0; odom.twist.twist.angular.z = vth;

&#xA0; &#xA0; //&#x53D1;&#x5E03;Odometry&#x6D88;&#x606F;
&#xA0; &#xA0; odom_pub.publish(odom);

&#xA0; &#xA0; last_time = current_time;
&#xA0; &#xA0; r.sleep();
&#xA0; }
}
</nav_msgs::odometry></nav_msgs></tf></ros>

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)
{

&#xA0; &#xA0; double x, y, z;
&#xA0; &#xA0; double roll, pitch, yaw;
&#xA0; &#xA0; x = msg->pose.pose.position.x;
&#xA0; &#xA0; y = msg->pose.pose.position.y;
&#xA0; &#xA0; z = msg->pose.pose.position.z;
&#xA0; &#xA0; tf::Quaternion quat; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; //&#x5B9A;&#x4E49;&#x4E00;&#x4E2A;&#x56DB;&#x5143;&#x6570;
&#xA0; &#xA0; tf::quaternionMsgToTF(msg->pose.pose.orientation, quat); //&#x53D6;&#x51FA;&#x65B9;&#x5411;&#x5B58;&#x50A8;&#x4E8E;&#x56DB;&#x5143;&#x6570;
&#xA0; &#xA0; tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

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

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

【完】

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

Original: https://www.cnblogs.com/CloudFlame/p/16443731.html
Author: CloudFlame
Title: 【cartographer_ros】四: 发布和订阅里程计odom信息