【学习记录】ros2中处理livox_ros_driver1格式的msg

livox的驱动版本有1和2两个，一些数据集是ros1版本，有些是ros2版本，导致自己的代码总是调来调去很麻烦。记录下解决办法。

我这里需要用到的，只有CustomMsg这个，而1和2两个的msg是一模一样的。

1、编译

首先创建一个工作空间

~/livox_msg_ws/

然后在下方创建只包含msg的package即可，不需要安装完整的SDK或者是driver（因为我不需要启动lidar）。
这个链接：https://github.com/LarryDong/livox_msg_ros2 ，之后编译，便出现了2版本的msg。

为了兼容1，可以把所有的数字2删掉（package.xml, CMakeLists.txt, 文件夹名称），然后重新编译，就出现了1。

2、运行rosbag

对于ros2版本，在终端中运行格式如下：

ros2 bag run <Your_db3_bag>

当然运行前需要source一下msg ws。如果ws只包含2的msg，而rosbag是1转过来的，则此时bag是无法识别livox_ros_driver/的消息类型的，会出现如下报错：

WARN] [1762862091.760178347] [rosbag2_player]: Ignoring a topic '/livox/lidar', reason: package 'livox_ros_driver' not found, searching: [/home/larry/livox_msgs_ws/install/livox_ros_driver2, /opt/ros/jazzy].

此时也不会发布这个topic的数据，因为不知道格式该如何解析。因为我们知道的是2格式的，这里的是1格式的。

所以，source一下包含1版本msg的ws之后，可以正常运行。

3、如何echo

此时我想echo一下这个topic是什么样子，同样，在source了msg的ws后，在终端执行：

ros2 topic echo /livox/lidar

会出现报错：

Cannot echo topic '/livox/lidar', as it contains more than one type: [livox_ros_driver/msg/CustomMsg, livox_ros_driver2/msg/CustomMsg]

这意味着终端发现两个 CustomMsg 格式，不知道用1还是2。

为了避免这个问题，把/livox/lidar这个 topic 显式的映射到 1 下方。采用如下方式运行

ros2 bag play 1018_00_LIO  --remap /livox/lidar:=/livox_ros_driver/livox/lidar

这样再另一个终端echo这个/livox_ros_driver下方的 msg 就不会有冲突了：

ros2 topic echo /livox_ros_driver/livox/lidar

4、自己的 SLAM 算法中如何同时handle两种topic？

在SLAM算法运行时，可以在一个ws下方粘贴过来1和2两个package，重新build一下这两个msg。
或者直接将上面msg的ws中build出来的install中的头文件粘过来不用重新编译，也行。问题不大（可能这种方式需要改一下缓存的问题？）

在具体的C++代码中，可以根据 lidarCallback 的类型选择livox_ros_driver还是2。

因为 CustomMsg可能同时使是livox1和2的，所以写代码时有些冲突。最终采用宏定义的方式进行。如果宏定义是版本1，则用1的版本。具体如下：

#define LIVOX_ROS_DRIVER 1 // or 2
#if LIVOX_ROS_DRIVER==1
        lidar_sub_ = this->create_subscription<livox_ros_driver::msg::CustomMsg>(
                    node_config.lidar_topic,                     // topic 名
                    rclcpp::SensorDataQoS(),                // QoS: 对激光雷达等高频传感器推荐用 SensorDataQoS()
                    std::bind(&LIONode::lidarCB_ros1, this, std::placeholders::_1)
                    );
#else 
        lidar_sub_ = this->create_subscription<livox_ros_driver2::msg::CustomMsg>(
                    node_config.lidar_topic,                     // topic 名
                    rclcpp::SensorDataQoS(),                // QoS: 对激光雷达等高频传感器推荐用 SensorDataQoS()
                    std::bind(&LIONode::lidarCB, this, std::placeholders::_1)
                    );
#endif

    imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
                node_config.imu_topic,                       // topic 名
                rclcpp::SensorDataQoS(),                // 同上
                std::bind(&LIONode::imuCB, this, std::placeholders::_1)
                );
    RCLCPP_INFO(this->get_logger(), "Subscribed to /livox/imu topic");
}

但调用时，可以将lidarCB_ros1这个函数调用ros2的版本。具体，修改msg的一些字段即可。如下：

// livox custom msg (ROS1's bag)
void LIONode::lidarCB_ros1(const livox_ros_driver::msg::CustomMsg::ConstSharedPtr msg) { 
    cout << "ros1 lidar callback" << endl; 
    
    // Initialize msg2 (ROS2 message)
    livox_ros_driver2::msg::CustomMsg msg2;
    
    // Copy fields directly
    msg2.header = msg->header;
    msg2.lidar_id = msg->lidar_id;
    msg2.point_num = msg->point_num;

    // If `rsvd` is a fixed array of 3 elements, copy it directly
    for (int i = 0; i < 3; ++i) {
        msg2.rsvd[i] = msg->rsvd[i];
    }

    // Copy each point from ROS1 message to ROS2 message
    msg2.points.resize(msg->points.size()); // Ensure msg2.points is the correct size
    for (size_t i = 0; i < msg->points.size(); ++i) {
        const auto& point1 = msg->points[i];
        auto& point2 = msg2.points[i];

        // Copy each field of CustomPoint
        point2.offset_time = point1.offset_time;
        point2.x = point1.x;
        point2.y = point1.y;
        point2.z = point1.z;
        point2.reflectivity = point1.reflectivity;
        point2.tag = point1.tag;
        point2.line = point1.line;
    }

    // Create a ConstSharedPtr to msg2 and call lidarCB with it
    auto msg2_shared = std::make_shared<livox_ros_driver2::msg::CustomMsg>(msg2);
    lidarCB(msg2_shared); // Pass the ConstSharedPtr to lidarCB
}