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

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 配置文件就可以.

相关博文介绍
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 层面.)