上一節介紹了里程計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資料的釋出和訂閱。