【cartographer_ros】四: 发布和订阅里程计odom信息
上一节介绍了激光雷达Scan传感数据的订阅和发布。
本节会介绍里程计Odom数据的发布和订阅。里程计在cartographer中主要用于前端位置预估和后端优化。
官方文档:
http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
目录
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数据的发布和订阅
【cartographer_ros】四: 发布和订阅里程计odom信息的更多相关文章
- 【cartogarpher_ros】三: 发布和订阅雷达scan信息
上一节介绍和测试了cartographer的官方demo. 本节会编写ros系统中,最常用的激光雷达LaserScan传感数据的订阅和发布,方便在cartographer中加入自己的数据进行建图与定位 ...
- SLAM+语音机器人DIY系列:(四)差分底盘设计——6.底盘里程计标
摘要 运动底盘是移动机器人的重要组成部分,不像激光雷达.IMU.麦克风.音响.摄像头这些通用部件可以直接买到,很难买到通用的底盘.一方面是因为底盘的尺寸结构和参数是要与具体机器人匹配的:另一方面是因为 ...
- 【cartographer_ros】五: 发布和订阅陀螺仪Imu信息
上一节介绍了里程计Odometry传感数据的订阅和发布. 本节会介绍陀螺仪Imu数据的发布和订阅.陀螺仪在cartographer中主要用于前端位置预估和后端优化. 目录 1:sensor_msgs/ ...
- 【cartographer_ros】六: 发布和订阅路标landmark信息
上一节介绍了陀螺仪Imu传感数据的订阅和发布. 本节会介绍路标Landmark数据的发布和订阅.Landmark在cartographer中作为定位的修正补充,避免定位丢失. 这里着重解释一下Land ...
- 视觉slam十四讲个人理解(ch7视觉里程计1)
参考博文::https://blog.csdn.net/david_han008/article/details/53560736 https://blog.csdn.net/n66040927/ar ...
- ROS中测试机器人里程计信息
在移动机器人建图和导航过程中,提供相对准确的里程计信息非常关键,是后续很多工作的基础,因此需要对其进行测试保证没有严重的错误或偏差.实际中最可能发生错误的地方在于机器人运动学公式有误,或者正负号不对, ...
- SLAM+语音机器人DIY系列:(三)感知与大脑——3.轮式里程计与运动控制
摘要 在我的想象中机器人首先应该能自由的走来走去,然后应该能流利的与主人对话.朝着这个理想,我准备设计一个能自由行走,并且可以与人语音对话的机器人.实现的关键是让机器人能通过传感器感知周围环境,并通过 ...
- 知方可补不足~SQL2008中的发布与订阅模式
回到目录 作用:完成数据库与数据库的数据同步 原理:源数据库发布需要同时的表,存储过程,或者函数:目标数据库去订阅它,当源发生变化时,目标数据库自己同步,注意,由于这个过程是SQL自动完成的,所以要求 ...
- redis 笔记06 发布与订阅、事务、慢查询日志、监视器
发布与订阅 1. 服务器状态在pubsub_channels字典保存了所有频道的订阅关系:SUBSCRIBE命令负责将客户端和被订阅的频道关联到这个字典里面,而UNSUBSCRIBE命令则负责 解除客 ...
随机推荐
- i2c调试工具分享
i2c-tools简介 在嵌入式开发仲,有时候需要确认硬件是否正常连接,设备是否正常工作,设备的地址是多少等等,这里我们就需要使用一个用于测试I2C总线的工具--i2c-tools. i2c-tool ...
- 源码解析Synchronous Queue 这种特立独行的队列
摘要:Synchronous Queue 是一种特立独行的队列,其本身是没有容量的,比如调用者放一个数据到队列中,调用者是不能够立马返回的,调用者必须等待别人把我放进去的数据消费掉了,才能够返回. 本 ...
- Bugku CTF练习题---MISC---眼见非实
Bugku CTF练习题---MISC---眼见非实 flag:flag{F1@g} 解题步骤: 1.观察题目,下载附件 2.拿到手以后发现是一个压缩包,打开是一个Word文档,观察其中的内容,除了开 ...
- Jenkins 自动化部署详解
一.背景 以前在发布系统的时候,总是需要经过本地合并代码 >编译 >发布到本地 >拷贝代码到远程服务器 >停止掉服务器上的应用池 >将要发布的文件替换原文件 >重启 ...
- 《Streaming Systems》第三章: Watermarks
定义 对于一个处理无界数据流的 pipeline 而言,非常需要一个衡量数据完整度的指标,用于标识什么时候属于某个窗口的数据都已到齐,窗口可以执行聚合运算并放心清理,我们暂且就给它起名叫 waterm ...
- 百度SEO算法技术的局限性,怎么做才能有收益
不知道大家有没有发现,我们使用百度的频率在减少,就算有时遇到一些问题,需要用百度来寻找答案,也会经常遇到搜索不到答案的情况.到底是出了什么问题?难道网络上的资源不够丰富了?浩如烟海的互联网,居然搜索不 ...
- 消息队列,IPC机制(进程间通信),生产者消费者模型,线程及相关
消息队列 创建 ''' Queue是模块multiprocessing中的一个类我们也可以这样导入from multiprocessing import Queue,创 建时queue = Queue ...
- mysql的命令二
1.插入数据 格式一:insert into table_name valuse (字段1,字段2): insert test1 values ('wangsan',22,'male'); 格式二:i ...
- Java synchronized那点事
前言 请看上篇:Java 对象头那点事 文章中的源码都有不同程度缩减,来源于openjdk8的开源代码(tag:jdk8-b120). 锁粗化过程 偏向锁 ①:markword中保存的线程ID是自己且 ...
- Nexus5x 修改Android开机动画
1.制作帧动画 这里随便从网上找了一个gif图片,导入PS中,打开后会形成很多帧图层,选择导航栏中的文件->脚本->将图层导出到文件可以将所有图层导出来.要注意文件命名,Android会按 ...