Title: Livox_ros_driver2 消息 (msg) 类型对 SLAM 应用程序的适配
文章目录
- I 前言
- II. 查看 ROS 消息
- III. Livox ROS 驱动的消息类型
- IV. 适配修改应用程序
- V. 总结
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 配置文件就可以.
相关博文介绍
Docker 环境下运行 Fast_LIO 进行三维建模的前/后处理设置
II. 查看 ROS 消息
如预料中那样, 使用 livox_ros_driver 是获得不了 HAP 的点云消息的, 本来也不支持 HAP.
使用 livox_ros_driver2 当然能拿到 HAP 的点云 topic “/livox/lidar”, 本来就支持 HAP. 且消息类型是 “livox_ros_drivers2/CustomMsg” (已在 .launch 文件中将参数 “xfer_format” 设置为 “1”).
而 FAST_LIO 虽然订阅了 “/livox/lidar”, 但消息类型却为 “livox_ros_drivers/CustomMsg”. 也就 fast_lio 兼容驱动 livox_ros_drivers, 而不兼容驱动 livox_ros_drivers2.
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” 的格式也是一样的.
两者就消息格式发布的内部代码实现也是类似的.
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 的修改:
同样地, 激光雷达和 IMU 初始标定的应用程序 LiDAR_IMU_Init (https://github.com/hku-mars/LiDAR_IMU_Init) 兼容 livox_ros_drivers2 的修改:
这样就可以硬件使用 Livox HAP 这些只和驱动 livox_ros_drivers2 兼容的新传感器, 继续学习原有开源 SLAM 程序了.
V. 总结
只是一个简单小技巧的记录,
关于 SLAM 应用程序适配新的机关雷达 Livox HAP 和新的 ROS 驱动 Livox_ros_driver2,
具体例子有 FAST_LIO 和 LiDAR_IMU_Init.
(有了 ROS 确实方便, 至少在 demo 层面.)
