本文已参与「新人创作礼」活动，一起开启掘金创作之路。
@TOC
一、查看 usb 挂载情况
USB通过串口接入IMU后
ls -l /dev/ttyUSB*
修改串口权限
sudo chmod 777 /dev/ttyUSB0
不过上面方法是临时的，每次启动都要输入一次，永久办法如下：
sudo gedit /etc/udev/rules.d/70-ttyusb.rules
内容输入：
KERNEL=="ttyUSB[0-9]*", MODE="0666"
二、安装serial包
sudo apt-get install ros-<distro>-serial
之后serial包会安装在opt/ros//share/serial目录下，然后
roscd serial
能够切换对应路径即为安装成功
三、创建ROS工程包（该过程可跳过）
mkdir -p ~/imu_ws/src
cd ~/imu_ws
catkin_make
cd src
catkin_create_pkg imu_com std_msgs roscpp serial
cd imu_com/src
touch imu_com.cpp
CmakeList.txt中添加：
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(imu_com src/imu_com.cpp)
target_link_libraries(imu_com ${catkin_LIBRARIES})
以下代码用于测试是否能正常读取串口数据 imu_com.cpp
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
//https://www.cnblogs.com/Braveliu/p/12219521.html
//用在C与C++混合编写的代码中，意为：如果是在C++下编译的
#ifdef __cplusplus
extern "C"{ //指示编译器大括号内这部分代码按C语言（而不是C++）的方式进行编译
//https://www.cnblogs.com/xiangtingshen/p/10980055.html
#endif
#define IMU_SERIAL "/dev/ttyUSB0"
#define BAUD (115200)
#define BUF_SIZE 1024
#ifdef __cplusplus
}
#endif
int main(int argc, char** argv)
{
int rev = 0;
//初始化节点
ros::init(argc, argv, "serial_imu");
//声明节点句柄
ros::NodeHandle n;
//声明串口对象
serial::Serial sp;
///设置串口属性
sp.setPort(IMU_SERIAL); //设置接口
sp.setBaudrate(BAUD); //设置波特率
//创建并设置超时时间timeout https://www.cnblogs.com/visionfeng/p/5614066.html
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
sp.setTimeout(to);
///打开串口
try
{
sp.open();
}
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port.");
return -1;
}
//判断串口是否打开成功
if(sp.isOpen())
{
//https://www.codeleading.com/article/67245272933/
ROS_INFO_STREAM(IMU_SERIAL << " is opened.");
}
else
{
return -1;
}
// alarm超时则产生SIGALRM信号。
// https://blog.csdn.net/qq_22863733/article/details/80349120
alarm(1);
//希望发布信息的速度为50Hz，要和 loop_rate.sleep() 配合使用
// https://blog.csdn.net/weixin_51060040/article/details/122238461
ros::Rate loop_rate(500);
while(ros::ok())
{
//获取缓冲区内的字节数
size_t num = sp.available();
if(num!=0)
{
uint8_t buffer[BUF_SIZE];
if(num > BUF_SIZE)
num = BUF_SIZE;
//读出数据
num = sp.read(buffer, num);
for (int i = 0; i < num; i++)
{
//16进制的方式打印到屏幕
std::cout << std::hex << (buffer[i] & 0xff) << " ";
}
std::cout << std::endl;
}
loop_rate.sleep();
}
//关闭串口
sp.close();
return 0;
}
四、基于开源串口程序修改
4.1 开源代码配置
由于没有找到我的IMU对应的开源ROS包，因此找了一个开源的代码（对应设备型号：HI226 HI229 CH100 CH110）进行修改，下面仅记录修改过程中的注意事项： 拷贝 products/examples/ROS/serial_imu_ws/下的所有内容
cd products/examples/ROS/serial_imu_ws/
catking_make
然后
cd products/examples/ROS/serial_imu_ws/
source ./devel/setup.bash
roslaunch imu_launch imu_msg.launch imu_package:=0x91
如果根据上面官方的说明运行一般是不会报错的，若你正好是它的设备（HI226 HI229 CH100 CH110），将会看到如下内容：
Devie ID: 0
Run times: 0 days 3:26:10:468
Frame Rate: 100Hz
Acc(G): 0.933 0.317 0.248
Gyr(deg/s): -0.02 0.30 -0.00
Mag(uT): 0.00 0.00 0.00
Eul(R P Y): 52.01 -66.63 -60.77
Quat(W X Y Z): 0.770 0.066 -0.611 -0.172
但很可惜我的不是，上面所有数据都输出为0。
4.2 根据自身状态数据包示例进行修改
首先是数据内容不一样，我的是：
而CH110对应的0x91类状态数据是：
因此要对应修改msg文件，即将Imu_data_package.msg：
uint8 tag
uint8 bitmap
uint8 id
float32 prs
uint32 time
uint32 frame_rate
float32 acc_x
float32 acc_y
float32 acc_z
float32 gyr_x
float32 gyr_y
float32 gyr_z
float32 mag_x
float32 mag_y
float32 mag_z
float32 eul_r
float32 eul_p
float32 eul_y
float32 quat_w
float32 quat_x
float32 quat_y
float32 quat_z
修改为：
uint8 tag
uint8 bitmap
uint8 id
float32 prs
uint32 time
uint32 frame_rate
float64 Latitude #纬度 (rad)
float64 Longitude #经度(rad)
float64 Height #高度(m)
float32 v_north #北向速度 (m/s)
float32 v_east #东向速度 (m/s)
float32 v_down #地向速度 (m/s)
float32 acc_x
float32 acc_y
float32 acc_z
float32 gravity #重力
float32 eul_r #横滚 (radians)
float32 eul_p #俯仰 (radians)
float32 eul_y #航向 (radians)
float32 gyr_x
float32 gyr_y
float32 gyr_z
#float32 mag_x
#float32 mag_y
#float32 mag_z
float32 Latitude_dev #纬度标准差 (m)
float32 Longitude_dev #经度标准差 (m)
float32 Height_dev #高度标准差 (m)
之后主要修改的就是serial_imu/src/ch_serial.c文件，我们自上而下屡(显示的是我根据自己IMU对应改过的内容）：
分别是两个字节的数据包帧头（CHSYNC1、CHSYNC2）， 和数据包的数据头字节数（CH_HDR_SIZE）
数据类型转换，我加了最后一行8字节数据合并成64位的double浮点数的代码
一般数据包都有CRC校验的内容，注意你的CRC规范（这里是CRC16-CCITT），和初值
/**
* @brief parse the payload of a frame and feed into data section (数据帧解析)
* */
static int parse_data(raw_t *raw)
{
int offset = 0, i = 0;
uint8_t *p = &raw->buf[CH_HDR_SIZE];
memset(raw->item_code, 0, sizeof(raw->item_code));
raw->nitem_code = 0;
while(offset < raw->len)
{
switch (raw->buf[1]) {
case IMU20:
/// 对应8.8.1 System State Packet /系统状态数据包
// 偏移照抄即可
raw->nimu = 1;
raw->item_code[raw->nitem_code++] = IMU20;
//TODO 还有4字节的系统、滤波器状态 和 4字节Unix时间秒 未读
raw->imu[0].timestamp = U4(p + offset + 8);
raw->imu[0].Latitude = R8(p+offset+12);
raw->imu[0].Longitude = R8(p+offset+20);
raw->imu[0].Height = R8(p+offset+28);
raw->imu[0].v_world[0] = R4(p + offset + 36);
raw->imu[0].v_world[1] = R4(p + offset + 40);
raw->imu[0].v_world[2] = R4(p + offset + 44);
raw->imu[0].acc[0] = R4(p + offset + 48);
raw->imu[0].acc[1] = R4(p + offset + 52);
raw->imu[0].acc[2] = R4(p + offset + 56);
raw->imu[0].gravity = R4(p + offset + 60);
raw->imu[0].eul[0] = R4(p + offset + 64);
raw->imu[0].eul[1] = R4(p + offset + 68);
raw->imu[0].eul[2] = R4(p + offset + 72);
raw->imu[0].gyr[0] = R4(p + offset + 76);
raw->imu[0].gyr[1] = R4(p + offset + 80);
raw->imu[0].gyr[2] = R4(p + offset + 84);
raw->imu[0].deviation[0] = R4(p + offset + 88);
raw->imu[0].deviation[1] = R4(p + offset + 92);
raw->imu[0].deviation[2] = R4(p + offset + 96);
offset += 100;
break;
// TODO 这部分有卫星信息
case IMU31:
raw->nimu = 1;
raw->item_code[raw->nitem_code++] = IMU31;
offset += 126;
break;
default:
offset++;
break;
}
}
return 1;
}
数据帧解析的函数，这里需要大改！
根据自己的校验规则进行修改。注意检查文档，查找CRC初值！！！
用于寻找数据包的帧头（CHSYNC1、CHSYNC2）
/**
* @brief 找到数据包头，并解析整个数据包
* @param raw 用于保存所读到的数据
* @param data 读到的一个字节的数据
* */
int ch_serial_input(raw_t *raw, uint8_t data)
{
/* synchronize frame */
// 如果是刚开始接收数据，则需要先找到数据包的数据头
if (raw->nbyte == 0)
{
if (!sync_ch(raw->buf, data)) return 0; // 寻找数据头
raw->nbyte = 3; //直到找到数据包的 1字节LRC校验位和2字节的帧头 之后才进行之后的操作
return 0;
}
raw->buf[raw->nbyte++] = data;
if (raw->nbyte == CH_HDR_SIZE) // 如果raw内数据足够一个数据头大小
{
raw->len = raw->buf[2];
if (raw->len > (MAXRAWLEN - CH_HDR_SIZE)) // 判断数据域长度是否过大
{
CH_TRACE("ch length error: len=%d\n",raw->len);
raw->nbyte = 0;
return -1;
}
}
// 在整个数据包读完之前，一直读新数据
if (raw->nbyte < (raw->len + CH_HDR_SIZE))
{
return 0;
}
// 整个数据包读完之后，重置nbyte，便于读下一个数据包
raw->nbyte = 0;
// 返回校验位是否正确
return decode_ch(raw);
}
这个是一个整体调用的函数：找到数据包帧头，进行校验，并解析整个数据包，将所读到的数据存到raw内
另外要根据自身数据包结构修改ch_serial.h的内容：
#define IMU20 (0x14)
#define IMU31 (0x1F)
typedef struct
{
uint32_t id; /* user defined ID */
uint32_t timestamp; //时间戳
double Latitude; //纬度 (rad)
double Longitude; //经度(rad)
double Height; //高度(m)
float v_world[3]; //北/东/地 三向速度(m/s)
float acc[3]; /* acceleration XYZ (m/s/s) */
float gravity; // 重力值（g）
float eul[3]; /* attitude: eular angle （deg）
* 横滚角(Roll),俯仰角(Pitch),航向角(Yaw)*/
float gyr[3]; /* angular velocity XYZ （deg/s）*/
// float mag[3]; /* magnetic field 磁强度 XYZ （uT）*/
float deviation[3]; //标准差 (m) 纬度/经度/高度
double quat[4]; /* attitude: quaternion */
// float pressure; /* air pressure （pa） */
}ch_imu_data_t;
typedef struct
{
int nbyte; /* number of bytes in message buffer */
int len; /* message length (bytes) */
uint8_t buf[MAXRAWLEN]; /* message raw buffer */
uint8_t gwid; /* network ID(HI222) */
uint8_t nimu; /* # of imu (HI222) */
ch_imu_data_t imu[MAX_NODE_SIZE]; /* imu data list, if (HI226/HI229/CH100/CH110, use imu[0]) */
uint8_t item_code[8]; /* item code recv in one frame 0x14/0x1F */
uint8_t nitem_code; /* # of item code */
}raw_t;
