【cartographer_ros】四： 发布和订阅里程计odom信息
上一节介绍了激光雷达Scan传感数据的订阅和发布。
本节会介绍里程计Odom数据的发布和订阅。里程计在cartographer中主要用于前端位置预估和后端优化。
官方文档：
目录
1：nav_msgs/Odometry消息类型
2：发布Odometry消息
3：订阅Odometry消息
1：nav_msgs/Odometry消息类型
在终端查看消息数据结构：
rosmsg show nav_msgs/OdometryOdometry消息类型数据结构如下：
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 rvizFixed 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数据的发布和订阅
手机扫一扫
移动阅读更方便
你可能感兴趣的文章