🚀 优质资源分享 🚀
| 学习路线指引（点击解锁） | 知识定位 | 人群定位 |
|---|---|---|
| 🧡 Python实战微信订餐小程序 🧡 | 进阶级 | 本课程是python flask+微信小程序的完美结合，从项目搭建到腾讯云部署上线，打造一个全栈订餐系统。 |
| 💛Python量化交易实战💛 | 入门级 | 手把手带你打造一个易扩展、更安全、效率更高的量化交易系统 |
上一节介绍了里程计Odometry传感数据的订阅和发布。
本节会介绍陀螺仪Imu数据的发布和订阅。陀螺仪在cartographer中主要用于前端位置预估和后端优化。
目录
1：sensor_msgs/Imu消息类型
2：发布Imu消息
3：订阅Imu消息
1：sensor_msgs/Imu消息类型
在终端查看消息数据结构：
rosmsg show sensor_msgs/ImuOdometry消息类型数据结构如下：
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 rvizFixed 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数据的发布和订阅。
