上一节介绍了cartographer中的TimedPointCloudData数据相关转换。

本节将介绍里程计Odom数据转换为cartographer中的OdometryData数据。还有cartographer中的OdometryData数据和Ros的nav_msgs::Odometry数据相互转换，方便引入Rosbag进行储存和读取。

1：OdometryData数据类型

2：Odom数据转换cartographer数据

3：Odom数据转换Ros数据

4：cartographer数据转换Ros数据

5：Ros数据转换cartographer数据

1：OdometryData数据类型

【odometry_data.h】中查看数据结构：

//时间
common::Time time;
//位置
transform::Rigid3d pose;

2：Odom数据转换cartographer数据

cartographer::sensor::OdometryData GetOdometryData()
{cartographer::sensor::OdometryData odometry_data;current_time = ros::Time::now();double dt = (current_time - last_time).toSec();double dx = (vx * cos(th) - vy * sin(th)) * dt;double dy = (vx * sin(th) + vy * cos(th)) * dt;double dth = vth * dt;last_time = current_time;x += dx;y += dy;th += dth;cartographer::common::Time curTime = cartographer::common::NowTime();const Eigen::AngleAxisd rotation(th, Eigen::Vector3d::UnitZ());const Eigen::Vector3d translation(x, y, 0.0);cartographer::transform::Rigid3d pose(translation, rotation);odometry_data = cartographer::sensor::OdometryData{curTime, pose};return odometry_data;
}

3：Odom数据转换Ros数据

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

4：cartographer数据转换Ros数据

nav_msgs::Odometry ToRosMsg(cartographer::sensor::OdometryData odometry_data)
{nav_msgs::Odometry odom;odom.header.stamp = TimeRosFromCarto(odometry_data.time);odom.header.frame_id = "odom";odom.pose.pose.position.x = odometry_data.pose.translation().x();odom.pose.pose.position.y = odometry_data.pose.translation().y();odom.pose.pose.position.z = odometry_data.pose.translation().z();odom.pose.pose.orientation.w = odometry_data.pose.rotation().w();odom.pose.pose.orientation.x = odometry_data.pose.rotation().x();odom.pose.pose.orientation.y = odometry_data.pose.rotation().y();odom.pose.pose.orientation.z = odometry_data.pose.rotation().z();odom.child_frame_id = "base_link";return odom;
}

5：Ros数据转换cartographer数据

cartographer::sensor::OdometryData ToCartoSensor(nav_msgs::Odometry odom)
{cartographer::sensor::OdometryData odometry_data;odometry_data.time = Time::CartoFromRos(odom.header.stamp);const Eigen::Vector3d translation(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z);const Eigen::Quaterniond rotation(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z);odometry_data.pose = cartographer::transform::Rigid3d(translation, rotation);return odometry_data;
}

【完】

下一节会介绍里程计Imu数据转换为cartographer中的cartographer::sensor::ImuData数据。

【cartographer without ros】四、里程计odom数据转换相关推荐

【cartographer_ros】四：发布和订阅里程计odom信息
上一节介绍了激光雷达Scan传感数据的订阅和发布. 本节会介绍里程计Odom数据的发布和订阅.里程计在cartographer中主要用于前端位置预估和后端优化. 官方文档: http://wiki.r ...
ROS机器人里程计校准
ROS机器人里程计校准麦克纳姆轮机器人里程计标定前提是你的机器人已经输入了正确的运动学模型并可以按照你给定的运动速度有正确的运动方向. 一.启动初始化节点首先打开第一个终端,打开你的机器人的初始 ...
ros(25):发布一个静态的里程计odom信息
有时为了验证程序可以运行,需要给程序输入一个里程计信息:有时为了在实车实验前验证程序,需要修改不同的里程计数据(可以在下面代码中增加一个odom_tmp的数组,或者用循环来使odom_tmp值变化), ...
ROS机器人里程计模型
3.5 移动机器人系统模型相信大家的机器人平台STM32端底层控制和机器人urdf建模都已经顺利完成了,在正式开始ros端编写机器人启动功能包之前,我们还不得不学习一些必要的理论知识.别担心数学不好 ...
《动手学机器人学》7.4机器人运动学介绍|机械臂运动学|两轮差速底盘运动学|轮式里程计
本系列教程作者:小鱼公众号:鱼香ROS QQ交流群:139707339 教学视频地址:小鱼的B站完整文档地址:鱼香ROS官网版权声明:如非允许禁止转载与商业用途. 7.4 机器人运动学介绍机器 ...
ROS察微【51】：如何将里程计和 IMU 与 robots_localization 融合
一.简述笔记是 ROS 开发人员 Live Class n.51 的附加材料,由 The Construct 的 Alberto Ezquerro 和 Ricardo Tellez 免费创建和提供 ...
基于ROS与C++，只利用IMU实现里程计和计步器，精确计算行走距离
老规矩,开门见山,先说说本篇文章实现了什么: 1.只使用IMU模块,实现了对于人行走距离的检测(精度在0.5m左右) 2.只使用IMU模块,通过峰值检测\机器学习两个方法,实现了一个精确的计步器背景 ...
ros移动机器人，激光雷达里程计rf2o_laser_odometry的使用与分析
目录 1.问题分析 2.激光雷达里程计 2.1 代码下载 2.2 使用方法 (1) ERRO:"base_link" passed to lookupTransform argum ...
odom 里程计轨迹的代码
在 C++ 中预测 odom 里程计轨迹的代码可能需要使用一些库来帮助处理数据和进行计算.以下是一个示例,它使用 Eigen 库来表示和操作矩阵和向量,以及使用 ROS (Robot Operatin ...

【cartographer without ros】四、里程计odom数据转换

1：OdometryData数据类型

2：Odom数据转换cartographer数据

3：Odom数据转换Ros数据

4：cartographer数据转换Ros数据

5：Ros数据转换cartographer数据

【cartographer without ros】四、里程计odom数据转换相关推荐

最新文章

热门文章