Livox_ros_driver2 消息 (msg) 类型对 SLAM 应用程序的适配

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);}