上一节介绍了里程计Odometry传感数据的订阅和发布。
本节会介绍陀螺仪Imu数据的发布和订阅。陀螺仪在cartographer中主要用于前端位置预估和后端优化。
目录
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 <ros/ros.h>
#include <tf/tf.h>
#include <sensor_msgs/Imu.h>
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];
//判断lrc
uint16_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<sensor_msgs::Imu>("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;
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";
}
else
cout << "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是自定义的串口通信类，附上代码:
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数据的发布和订阅。
