Title: Livox_ros_driver2 消息 (msg) 类型对 SLAM 应用程序的适配
I 前言
有时候, 拿到最新的 Livox 激光传感器, 比如 HAP, 原厂也提供了 ROS 驱动支持 livox_ros_drivers2 (https://github.com/Livox-SDK/livox_ros_driver2).
但是之前的开源 SLAM 代码 (比如 FAST_LIO, https://github.com/hku-mars/FAST_LIO) 还没支持 livox_ros_drivers2 呢, 都还只支持 livox_ros_driver (https://github.com/Livox-SDK/livox_ros_driver).
这里简单记录一下这个适配过程.
要让老应用程序 (FAST_LIO) 适配新传感器 (Livox HAP) 包含驱动的适配、消息的适配、传感器参数配置的适配等. 这里只涉及驱动和消息的适配. 传感器参数适配只需按照实际参数值修改应用程序的 .yaml 配置文件就可以.
II. 查看 ROS 消息
如预料中那样, 使用 livox_ros_driver 是获得不了 HAP 的点云消息的, 本来也不支持 HAP.
![driver1-Null-topic](https://i-blog.csdnimg.cn/blog_migrate/0dc75fe3b09d4f1241bcceedeac30e2d.png#pic_center)
使用 livox_ros_driver2 当然能拿到 HAP 的点云 topic “/livox/lidar”, 本来就支持 HAP. 且消息类型是 “livox_ros_drivers2/CustomMsg” (已在 .launch 文件中将参数 “xfer_format” 设置为 “1”).
![driver2-topic1](https://i-blog.csdnimg.cn/blog_migrate/6b021ea44f844846f10affbd83ea4eb3.png#pic_center)
而 FAST_LIO 虽然订阅了 “/livox/lidar”, 但消息类型却为 “livox_ros_drivers/CustomMsg”. 也就 fast_lio 兼容驱动 livox_ros_drivers, 而不兼容驱动 livox_ros_drivers2.
![fast_lio_msg](https://i-blog.csdnimg.cn/blog_migrate/0483f3f4871852110475e90249ea9346.png#pic_center)
III. Livox ROS 驱动的消息类型
不管是 livox_ros_drivers 还是 livox_ros_drivers2 都能通过 .launch 文件配置激光点云的消息类型. 这里只用到 “xfer_format” 的可选参数 “1”, 即 Livox 自定义点云格式.
Parameter | Detailed description | Default |
---|---|---|
xfer_format | Set pointcloud format 0 —— Livox pointcloud2(PointXYZRTLT) pointcloud format 1 —— Livox customized pointcloud format 2 —— Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library | 0 |
事实上, 两个版本驱动的 “Livox customized pointcloud format” 的格式也是一样的.
![driver1-2的msg格式](https://i-blog.csdnimg.cn/blog_migrate/4f8525e632859e43f6ee6fc91e199d31.png#pic_center)
两者就消息格式发布的内部代码实现也是类似的.
Livox_ros_driver 中 livox_ros_driver/livox_ros_driver/lddc.cpp 的内部实现
*pub = new ros::Publisher;
if (kPointCloud2Msg == transfer_format_) {
**pub =
cur_node_->advertise<sensor_msgs::PointCloud2>(name_str, queue_size);
ROS_INFO(
"%s publish use PointCloud2 format, set ROS publisher queue size %d",
name_str, queue_size);
} else if (kLivoxCustomMsg == transfer_format_) {
**pub = cur_node_->advertise<livox_ros_driver::CustomMsg>(name_str,
queue_size);
ROS_INFO(
"%s publish use livox custom format, set ROS publisher queue size %d",
name_str, queue_size);
} else if (kPclPxyziMsg == transfer_format_) {
**pub = cur_node_->advertise<PointCloud>(name_str, queue_size);
ROS_INFO(
"%s publish use pcl PointXYZI format, set ROS publisher queue "
"size %d",
name_str, queue_size);
}
Livox_ros_driver2 中 src/lddc.cpp 的内部实现
*pub = new ros::Publisher;
if (kPointCloud2Msg == transfer_format_) {
**pub =
cur_node_->GetNode().advertise<sensor_msgs::PointCloud2>(name_str, queue_size);
DRIVER_INFO(*cur_node_,
"%s publish use PointCloud2 format, set ROS publisher queue size %d",
name_str, queue_size);
} else if (kLivoxCustomMsg == transfer_format_) {
**pub = cur_node_->GetNode().advertise<livox_ros_driver2::CustomMsg>(name_str,
queue_size);
DRIVER_INFO(*cur_node_,
"%s publish use livox custom format, set ROS publisher queue size %d",
name_str, queue_size);
} else if (kPclPxyziMsg == transfer_format_) {
**pub = cur_node_->GetNode().advertise<PointCloud>(name_str, queue_size);
DRIVER_INFO(*cur_node_,
"%s publish use pcl PointXYZI format, set ROS publisher queue "
"size %d",
name_str, queue_size);
}
IV. 适配修改应用程序
有了上面的分析, 可知只需修改应用程序的依赖、头文件、数据类型就可以了. 直白一点就是在应用程序中遇见 “livox_ros_driver” 全替换为 “livox_ros_driver2” 就行了.
SLAM 前端应用程序 FAST_LIO 兼容 livox_ros_drivers2 的修改:
![fast_lio中修改](https://i-blog.csdnimg.cn/blog_migrate/373476aedde9704d606c7c11e01bb472.png#pic_center)
同样地, 激光雷达和 IMU 初始标定的应用程序 LiDAR_IMU_Init (https://github.com/hku-mars/LiDAR_IMU_Init) 兼容 livox_ros_drivers2 的修改:
![lidar_IMU_Init中修改](https://i-blog.csdnimg.cn/blog_migrate/71fbfaaad69fd7307ec39f3aa595e2cd.png#pic_center)
这样就可以硬件使用 Livox HAP 这些只和驱动 livox_ros_drivers2 兼容的新传感器, 继续学习原有开源 SLAM 程序了.
V. 总结
只是一个简单小技巧的记录,
关于 SLAM 应用程序适配新的机关雷达 Livox HAP 和新的 ROS 驱动 Livox_ros_driver2,
具体例子有 FAST_LIO 和 LiDAR_IMU_Init.
(有了 ROS 确实方便, 至少在 demo 层面.)