【cartographer_ros】五: 发布和订阅陀螺仪Imu信息

🚀 优质资源分享 🚀

学习路线指引(点击解锁)知识定位人群定位
🧡 Python实战微信订餐小程序 🧡进阶级本课程是python flask+微信小程序的完美结合,从项目搭建到腾讯云部署上线,打造一个全栈订餐系统。
💛Python量化交易实战💛入门级手把手带你打造一个易扩展、更安全、效率更高的量化交易系统

上一节介绍了里程计Odometry传感数据的订阅和发布。

本节会介绍陀螺仪Imu数据的发布和订阅。陀螺仪在cartographer中主要用于前端位置预估和后端优化。

目录

1:sensor_msgs/Imu消息类型

2:发布Imu消息

3:订阅Imu消息


1:sensor_msgs/Imu消息类型

在终端查看消息数据结构:

rosmsg show sensor_msgs/Imu

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

Header header
geometry\_msgs/Quaternion orientation
float64[9] orientation\_covariance // Row major about x, y, z axes
geometry\_msgs/Vector3 angular\_velocity
float64[9] angular\_velocity\_covariance // Row major about x, y, z axes
geometry\_msgs/Vector3 linear\_acceleration
float64[9] linear\_acceleration\_covariance // Row major x, y z

其中linear_acceleration表示线加速度,angular_velocity表示角速度,orientation表示姿态,使用四元数表示。covariance表示对应协方差,体现各个数据的误差


2:发布Imu消息

陀螺仪用的是LPMS-IG1 RS232,这个陀螺仪同时能提供角速度 ,线加速度,和欧拉角。

#include 
#include 
#include using namespace std;unsigned int step = 0;
unsigned int data_i = 0;
unsigned int data_len = 0;
unsigned char handle_buf[2048];float acc[3];
float gyo[3];
float eular[3];void DataReceivedCallback(std::vector<unsigned char> &data)
{unsigned char datasingle1;for (size\_t k = 0; k < data.size(); k++){datasingle1 = data[k];switch (step){case 0:{if (datasingle1 == 0x3A){step = 1;data_i = 0;memset(handle_buf, 0, 2048);}break;}case 1: // sensor id low{handle_buf[0] = datasingle1;step = 2;break;}case 2: // sensor id high{handle_buf[1] = datasingle1;step = 3;break;}case 3: //指令号 low{handle_buf[2] = datasingle1;step = 4;break;}case 4: //指令号 high{handle_buf[3] = datasingle1;step = 5;break;}case 5: //数据长度 low{handle_buf[4] = datasingle1;data_len = datasingle1;step = 6;break;}case 6: //数据长度 high{handle_buf[5] = datasingle1;data_len += (uint16\_t)handle_buf[5] * 256;if (data_len > 512){step = 0;cout << " data\_len error : " << hex << datasingle1 << ", " << data_len << std::endl;}else{if (data_len > 0){data_i = 0;step = 7;}else{step = 0;cout << " data\_len error : " << hex << datasingle1 << ", " << data_len << std::endl;}}break;}case 7:{handle_buf[data_i + 6] = datasingle1;data_i++;if (data_i >= data_len + 4) //完整一帧{//判断包尾if ((handle_buf[data_len + 8] != 0x0D) && (handle_buf[data_len + 9] != 0x0A)){step = 0;cout << " tail error : " << hex << handle_buf[data_len + 8] << ", " << hex << handle_buf[data_len + 9] << std::endl;break;}uint16\_t lrc = ((uint16\_t)handle_buf[data_len + 7] * 256) + (uint16\_t)handle_buf[data_len + 6];//判断lrcuint16\_t sum_lrc = 0;for (unsigned int i = 0; i < (6 + data_len); i++){sum_lrc += handle_buf[i];}if (lrc != sum_lrc){step = 0;cout << " crc error : " << lrc << ", " << sum_lrc << std::endl;break;}//线加速度(含重力)acc[0] = *((float *)&handle_buf[22]);acc[1] = *((float *)&handle_buf[26]);acc[2] = *((float *)&handle_buf[30]);//角速度(陀螺仪I的输出)gyo[0] = *((float *)&handle_buf[82]);gyo[1] = *((float *)&handle_buf[86]);gyo[2] = *((float *)&handle_buf[90]);//欧拉角eular[0] = *((float *)&handle_buf[146]);eular[1] = *((float *)&handle_buf[150]);eular[2] = *((float *)&handle_buf[154]);step = 0;}break;}default:break;}}
}int main(int argc, char **argv)
{ros::init(argc, argv, "Imu\_publisher");ros::NodeHandle n;ros::Publisher imu_pub = n.advertise("imu", 50);string device = "/dev/ttyUSB0";int baud\_rate = 921600;int data\_bits = 8;int stop\_bits = 0;string parity = "n";boost::shared\_ptr serialPort;serialPort.reset(new SerialPort(device, baud\_rate, data\_bits, stop\_bits, parity));auto binding = bind(&DataReceivedCallback, this, std::placeholders::\_1);serialPort->setcallback(binding);if (serialPort->Open()){serialPort->LoadConfig();cout << "Init serial open success";}elsecout << "Init serial open false";ros::Rate r(1.0);while (n.ok()){ros::spinOnce();sensor\_msgs::Imu imu;imu.header.stamp = ros::Time::now();imu.header.frame\_id = "base\_link";imu.linear\_acceleration.x = acc[0] * (-9.8);imu.linear\_acceleration.y = acc[1] * (-9.8);imu.linear\_acceleration.z = acc[2] * (-9.8);imu.angular\_velocity.x = gyo[0] * 3.1415926 / 180.0;imu.angular\_velocity.y = gyo[1] * 3.1415926 / 180.0;imu.angular\_velocity.z = gyo[2] * 3.1415926 / 180.0;imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(eular[0], eular[1], eular[2]);//发布Imu消息imu\_pub.publish(imu);last\_time = current\_time;r.sleep();}
}
折叠 

SerialPort是自定义的串口通信类,附上代码:

SerialPort.h

SerialPort.cpp


3:订阅Imu消息

(1) 通过rosbag订阅

rostopic echo /imu

(2) 通过rviz查看
打开rviz

rosrun rviz rviz

Fixed Frame修改为base_link,添加Imu并将Topic设为/imu

(3) 编写程序打印

#include "ros/ros.h"
#include "sensor\_msgs/Imu.h"void imuCallback(const sensor_msgs::Imu::ConstPtr &msg)
{ROS\_INFO("imu: %f, %f, %f, %f, %f, %f, %f, %f, %f, %f", msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z,msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
}int main(int argc, char **argv)
{ros::init(argc, argv, "listener");ros::NodeHandle node;ros::Subscriber subimu = node.subscribe("imu", 1000, imuCallback);ros::spin();return 0;
}

【完】


下一节会介绍路标Landmark数据的发布和订阅。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/400745.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

css reset

最近在想把自己写的demo写成一个完整的项目&#xff0c;在写css reset的时候想参考别人的css reset是怎么来写的&#xff0c;也有想直接下载normalize.css这样的css reset直接用。但是看了张神写的对css reset的重新审视受益匪浅&#xff0c;文章发布的比较早原文地址http://ww…

MSP432P401R TI Drivers 库函数学习笔记(八)ADC

目录API (机翻)上机实战引脚配置ADC引脚配置串口引脚配置指示工作状态的LED1引脚配置代码部分ADC初始化和读取函数myADC.cmyADC.h获取数据并通过串口发送main.cmain.h任务管理函数myTask.cmyTask.h串口代码myUart.cmyUart.h实验结果平台&#xff1a;Code Composer Studio 10.4.…

PHP 杂谈《重构-改善既有代码的设计》之三 重新组织数据

介绍承接上文的PHP 杂谈《重构-改善既有代码的设计》之 重新组织你的函数继续重构方面的内容。这章主要针对数据的重构。1、争论的声音——直接访问Field还是通过函数&#xff08;Accessor&#xff09;访问Field2.修改Array为Object&#xff1a;当你看到一个Array很像一个数据结…

轻松获取LAMP环境的配置参数

大家是否遇到过去了新公司&#xff0c;公司内的LAMP&#xff0c;LNMP等所有的环境都是配置好的&#xff08;已经在提供服务了&#xff09;&#xff0c;公司又没有留下部署文档&#xff0c;甚至安装LAMP&#xff0c;LAMP等环境的人已经和你交接完离职了&#xff0c;那么线上服务…

Python迷宫生成器

&#x1f680; 优质资源分享 &#x1f680; 学习路线指引&#xff08;点击解锁&#xff09;知识定位人群定位&#x1f9e1; Python实战微信订餐小程序 &#x1f9e1;进阶级本课程是python flask微信小程序的完美结合&#xff0c;从项目搭建到腾讯云部署上线&#xff0c;打造一…

数组的操作

有关操作数组的方法(函数): (常用)push() 从尾部添加新元素 操作的是原数组 pop() 从尾部删除一个元素 unshift() 从开头添加元素 shift() 从开头删除元素 slice(start, end) 获取数组中某些元素&#xff0c;返回一个新数组&#xff1b;从start下标位置起开始截取到end下标(不…

MSP430F5529 DriverLib 库函数I2C驱动OLED屏幕

目录I2C驱动代码MSP430F5529_I2C.cMSP430F5529_I2C.hOLED初始化及测试OLED驱动代码OLED.cOLED.hOLED_Font.h显示效果平台&#xff1a;Code Composer Studio 10.4.0 MSP430F5529 LaunchPad™ Development Kit (MSP‑EXP430F5529LP) I2C驱动代码 P3.0为SDA&#xff0c;P3.1为S…

java经典算法四十题

java经典算法四十题 【程序8】题目&#xff1a;求saaaaaaaaaaaa...a的值&#xff0c;其中a是一个数字。例如222222222222222(此时共有5个数相加)&#xff0c;几个数相加有键盘控制。1.程序分析&#xff1a;关键是计算出每一项的值。import java.io.*; public class Sumloop { p…

html调用servlet(JDBC在Servlet中的使用)(2)

5.修改数据 5.1编写查询条件页面 修改单条数据的时候&#xff0c;首先是查询出单个数据的详细信息&#xff0c;然后根据实际需要部分修改或者全部修改。修改之后&#xff0c;数据会提交到数据库&#xff0c;数据库中保存更新以后的数据。 查询出单条数据的查询条件页面代码如下…

gulp基础使用方法记录

一、开始 使用gulp&#xff0c;需知道4个API&#xff1a; gulp.task():用来定义任务, 格式&#xff1a;gulp.task(name[, deps], fn) name 为任务名 deps 是当前定义的任务需要依赖的其他任务&#xff0c;为一个数组。当前定义的任务会在所有依赖的任务执行完毕后才开始执行。如…

dolphinscheduler简单任务定义及复杂的跨节点传参

&#x1f680; 优质资源分享 &#x1f680; 学习路线指引&#xff08;点击解锁&#xff09;知识定位人群定位&#x1f9e1; Python实战微信订餐小程序 &#x1f9e1;进阶级本课程是python flask微信小程序的完美结合&#xff0c;从项目搭建到腾讯云部署上线&#xff0c;打造一…

【 Grey Hack 】万金油脚本:常见端口漏洞检测

目录脚本源码用法效果及示例SSH 端口FTP 端口版本&#xff1a;Grey Hack v0.7.3618 - Alpha 适用于SSH (22) 端口、FTP (21) 端口、HTTP (80) 端口、SMTP (25) 端口及3306/3307 端口 。 脚本源码 if params.len ! 2 or params[0] "-h" or params[0] "--help…

JUC源码学习笔记1——AQS和ReentrantLock

&#x1f680; 优质资源分享 &#x1f680; 学习路线指引&#xff08;点击解锁&#xff09;知识定位人群定位&#x1f9e1; Python实战微信订餐小程序 &#x1f9e1;进阶级本课程是python flask微信小程序的完美结合&#xff0c;从项目搭建到腾讯云部署上线&#xff0c;打造一…

Android 应用内直接跳转酷市场

不确定酷市场后期是否还会该包名或者路径,目前的7.9 版本测试通过. private void gotoCoolapkMarket() {try {Intent intent new Intent(Intent.ACTION_VIEW);intent.setData(Uri.parse("market://details?id" getPackageName()));intent.setClassName("com.…

【 Grey Hack 】万金油脚本:常见端口获取shell

目录脚本源码用法效果及示例成功示例FTP &#xff08;21&#xff09;端口HTTP &#xff08;80&#xff09;端口失败示例版本&#xff1a;Grey Hack v0.7.3618 - Alpha 适用于SSH (22) 端口、FTP (21) 端口、HTTP (80) 端口、SMTP (25) 端口及3306/3307 端口。 脚本源码 if pa…

Date 类型转换

Date类型默认固定输出格式是 “Fri Apr 12 17:26:23 CST 2013” 该类型属性本身无法改变格式&#xff0c;要想在前台jsp等页面呈现出你想要的格式就需要改变类型。 Date 转换成 String      转换成其它如“YYYY-MM-DD”,"YYYY-MM-DD hh:mm:ss"等之类的格式 Si…

ECharts整合HT#160;for#160;Web的网络拓扑图应用

ECharts图形组件在1.0发布的时候我就已经有所关注&#xff0c;今天在做项目的时候遇到了图标的需求&#xff0c;在HT for Web上也有图形组件的功能&#xff0c;但是在尝试了下具体实现后&#xff0c;发现HT for Web的图形组件是以矢量的格式来呈现的&#xff0c;在展现上可以有…

一个月后,我们又从 MySQL 双主切换成了主 - 从!

&#x1f680; 优质资源分享 &#x1f680; 学习路线指引&#xff08;点击解锁&#xff09;知识定位人群定位&#x1f9e1; Python实战微信订餐小程序 &#x1f9e1;进阶级本课程是python flask微信小程序的完美结合&#xff0c;从项目搭建到腾讯云部署上线&#xff0c;打造一…

如何进行有效的沟通----日常沟通及会议

日常开发和工作中会经常遇到沟通不畅的问题&#xff0c;"communication" 不论是在学术还是实践中都是一个很重要的议题。因为低效的沟通造成的开发事故有时候是灾难性的。 沟通的问题为认为本质上就是信息不对称。下面以日常沟通及会议沟通做一些浅显的讨论&#xff…

【 Grey Hack 】万金油脚本:常见端口获取Password

目录脚本源码用法效果及示例SSH &#xff08;80&#xff09;端口FTP &#xff08;21&#xff09;端口HTTP &#xff08;80&#xff09;端口失败示例SMTP &#xff08;25&#xff09;端口版本&#xff1a;Grey Hack v0.7.3618 - Alpha 适用于SSH (22) 端口、FTP (21) 端口、HTTP…