Mid70 接外置imu跑PointLIO算法（ubuntu22.04 ROS2 humble）

一、修改CMAkeLists.txt package.xml

        将CMakeLists和package.xml里面的 livox_ros_driver2 全部替换为 livox_interfaces，package.xml添加

find_package(livox_interfaces REQUIRED)

        CMAkeLists.txt中改为使用C++17（我用的是humble）

ADD_COMPILE_OPTIONS(-std=c++14)
ADD_COMPILE_OPTIONS(-std=c++14)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
# ->
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_FLAGS "-O3")

set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
# ->
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions")

二、修改PointLIO源码

        找到 Point-LIO/src/preprocess.h 文件，修改

#include <livox_ros2_driver/msg/custom_msg.hpp>
# ->
#include <livox_interfaces/msg/custom_msg.hpp>

void process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
# ->
void process_cut_frame_livox(const livox_interfaces::msg::CustomMsg::SharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);

void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
# ->
void process(const livox_interfaces::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);

void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg);
# ->
void avia_handler(const livox_interfaces::msg::CustomMsg::SharedPtr &msg);

        找到 Point-LIO/src/preprocess.cpp 文件，修改

const livox_ros_driver2::msg::CustomMsg::SharedPtr & msg, PointCloudXYZI::Ptr & pcl_out)
# ->
const livox_interfaces::msg::CustomMsg::SharedPtr & msg, PointCloudXYZI::Ptr & pcl_out)

void Preprocess::process_cut_frame_livox(
const livox_ros_driver2::msg::CustomMsg::SharedPtr & msg, deque<PointCloudXYZI::Ptr> & pcl_out,
deque<double> & time_lidar, const int required_frame_num, int scan_count)
# ->
void Preprocess::process_cut_frame_livox(
const livox_interfaces::msg::CustomMsg::SharedPtr & msg, deque<PointCloudXYZI::Ptr> & pcl_out,
deque<double> & time_lidar, const int required_frame_num, int scan_count)

void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr & msg)
# ->
void Preprocess::avia_handler(const livox_interfaces::msg::CustomMsg::SharedPtr & msg)

        找到 Point-LIO/src/parameters.h 文件，修改

#include <livox_ros_driver2/msg/custom_msg.hpp>
# ->
#include <livox_interfaces/msg/custom_msg.hpp>

        找到 Point-LIO/src/li_initialization.h 文件，修改

void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::SharedPtr & msg);
# ->
void livox_pcl_cbk(const livox_interfaces::msg::CustomMsg::SharedPtr & msg);

        找到 Point-LIO/src/li_initialization.cpp 文件，修改

void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::SharedPtr & msg)
# ->
void livox_pcl_cbk(const livox_interfaces::msg::CustomMsg::SharedPtr & msg)

        找到 Point-LIO/src/laserMapping.cpp 文件，修改

rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr sub_pcl_livox;
if (p_pre->lidar_type == AVIA) {
sub_pcl_livox = nh->create_subscription<livox_ros_driver2::msg::CustomMsg>(
lid_topic, rclcpp::SensorDataQoS(),
[](const livox_ros_driver2::msg::CustomMsg::SharedPtr msg) { livox_pcl_cbk(msg); });
}
# ->
rclcpp::Subscription<livox_interfaces::msg::CustomMsg>::SharedPtr sub_pcl_livox;
if (p_pre->lidar_type == AVIA) {
sub_pcl_livox = nh->create_subscription<livox_interfaces::msg::CustomMsg>(
lid_topic, rclcpp::SensorDataQoS(),
[](const livox_interfaces::msg::CustomMsg::SharedPtr msg) { livox_pcl_cbk(msg); });
}

三、根据给出配置文件编写Mid70的启动配置文件，更改 ROS2 驱动实现时间戳软同步

        将mid360配置文件复制下来，改为下方配置

/**:
ros__parameters:
time_sync_en: true
use_imu_as_input: True
prop_at_freq_of_imu: True
check_satu: True
init_map_size: 10
point_filter_num: 3
space_down_sample: True
filter_size_surf: 0.3
filter_size_map: 0.3
ivox_nearby_type: 6
runtime_pos_log_enable: False

common:
lid_topic: "/livox/lidar"
imu_topic: "/imu/data_ex_raw"
con_frame: False
con_frame_num: 1
cut_frame: False
cut_frame_time_interval: 0.1
time_diff_lidar_to_imu: -0.0003

preprocess:
lidar_type: 1
scan_line: 6
timestamp_unit: 3
blind: 0.5

mapping:
imu_en: True
extrinsic_est_en: True
imu_time_inte: 0.005
lidar_time_inte: 0.1
satu_acc: 3.0
satu_gyro: 35.0
acc_norm: 1.0
lidar_meas_cov: 0.01
acc_cov_output: 500.0
gyr_cov_output: 1000.0
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.05
imu_meas_omg_cov: 0.05
gyr_cov_input: 0.01
acc_cov_input: 0.1
plane_thr: 0.1
match_s: 81.0
ivox_grid_resolution: 2.0
gravity: [ 0.0, 0.0, -9.810 ]
gravity_init: [ 0.0, 0.0, -9.810 ]
extrinsic_T: [ 0.0, 0.0, 0.0 ]
extrinsic_R: [ 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 ]

odometry:
publish_odometry_without_downsample: False

publish:
path_en: True
scan_publish_en: True
scan_bodyframe_pub_en: False
tf_send_en: True

pcd_save:
pcd_save_en: False
interval: -1

        找到 livox_ros2_driver/livox_ros2_driver/livox_ros2_driver/lddc.cpp 文件，修改

/** convert to ros time stamp */
livox_msg.header.stamp = rclcpp::Time(timestamp);
# ->
// 使用ROS2当前时间戳实现软同步
livox_msg.header.stamp = rclcpp::Clock().now();

四、启动

        先编译ROS2驱动包

        运行 imu 驱动包，需注意也得使用ROS2时间戳

        运行雷达 ROS2 驱动包 (注意只能运行这个）

ros2 launch livox_ros2_driver livox_lidar_msg_launch.py

        运行 PointLIO

ros2 launch point_lio point_lio.launch.py

        然后你就会看到 —— 什么都没有

        先别急，让我们看看tf

ls@ls-OMEN-Gaming-Laptop-16-ae0xxx:~/Desktop/FoundationPose/src$ $ rosrun tf2_ros tf2_echo camera_init aft_mapped
[INFO] [1769851157.987689281] [tf2_echo]: Waiting for transform camera_init -> aft_mapped: Invalid frame ID "camera_init" passed to canTransform argument target_frame – frame does not exist
At time 1769851158.926205873
– Translation: [1711.428, -916.227, 5995.590]
– Rotation: in Quaternion (xyzw) [0.079, 0.384, -0.360, 0.847]
– Rotation: in RPY (radian) [-0.203, 0.785, -0.888]
– Rotation: in RPY (degree) [-11.637, 45.003, -50.890]
– Matrix:
0.446 0.670 0.593 1711.428
-0.549 0.729 -0.410 -916.227
-0.707 -0.143 0.693 5995.590
0.000 0.000 0.000 1.000
At time 1769851159.914621829
– Translation: [2133.416, -1159.996, 6954.824]
– Rotation: in Quaternion (xyzw) [0.088, 0.416, -0.394, 0.815]
– Rotation: in RPY (radian) [-0.281, 0.845, -1.027]
– Rotation: in RPY (degree) [-16.122, 48.387, -58.867]
– Matrix:
0.343 0.715 0.609 2133.416
-0.568 0.674 -0.471 -1159.996
-0.748 -0.184 0.638 6954.824
0.000 0.000 0.000 1.000

        破案了，偏移1711.428m

五、优化

        1、笔者没有测量imu和雷达之间的偏移，T和R矩阵都设置为无偏移

        2、暴力时间戳软同步精度不够

        3、imu没设置协方差