1. 前言

在CRV调试中，我采用 “Docker 跑驱动(Ubuntu 18.04) + 宿主机跑算法(Ubuntu 20.04)” 的架构，以解决 ROS 版本不兼容问题（crv底盘驱动依赖 Melodic，而算法依赖 Noetic）。

本文默认已满足以下前置条件：

• 网络已打通：Docker 容器已通过 --net=host 模式启动，容器内的 Hesai 驱动与宿主机的 roscore 通信正常。

• 驱动已运行：在宿主机使用 rostopic list 可以查看到雷达点云话题。

• 算法已下载：宿主机已 clone 了 LeGO-LOAM 代码包。

2. CRV TF 坐标系变换 (Static TF)

LeGO-LOAM 及其后的建图算法都需要维护一个正确的 TF 树。对于车辆底盘（CRV），我们通常以 后轮轴中心 作为基准坐标系 (base_link)。

2.1 测量数据准备

人工测量雷达相对于 CRV 后轮轴中心的 XYZ 偏移量。确认以下数据定义：

• X轴：车头方向为正（前正后负）。

• Y轴：车身左侧方向为正（左正右负）。

• Z轴：垂直地面向上为正（上正下负）。

假设测量结果为： 雷达安装在后轴中心的前方 2.5 米，左右居中，距离地面高度 1.5 米。 即：x=2.5, y=0.0, z=1.5。

2.2 编写 TF 发布器

我们需要在算法启动前，发布一个静态坐标变换（Static Transform），将雷达坐标系挂载到车体坐标系上。

建议直接修改 LeGO-LOAM 的 run.launch 文件（位于 LeGO-LOAM/launch/run.launch），在 <launch> 标签内添加如下节点：

<launch>
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_lidar" 
          args="2.5 0.0 1.5 0 0 0 base_link hesai_lidar 100" />

</launch>

原理解析： 这段代码告诉 ROS 系统：“雷达数据虽然是在 hesai_lidar 坐标系下产生的，但它固定在 base_link 前方 2.5米处。” 这样算法在进行运动补偿（IMU去畸变）和地图构建时，轨迹才会基于车辆后轴，而不是雷达中心。

3. 算法适配：针对 Hesai 雷达修改 LeGO-LOAM

LeGO-LOAM 原生支持 Velodyne VLP-16，对于 Hesai 雷达（尤其是多线束，如 32线/64线/128线），必须修改 utility.h 文件。

打开 LeGO-LOAM/include/utility.h：

3.1 关键参数修改

// 1. 修改雷达线束 (根据您的雷达型号)
// 建议：如果不需要极致精度，可在驱动层降采样输出，或者在此处配合 stride 使用。
extern const int N_SCAN = 128; // 假设您使用的是 Pandar XT-32，如果是128线请填128

// 2. 修改水平分辨率
// Hesai 雷达通常为 1800 或 2000，取决于您的驱动转速设置 (10Hz/20Hz)
extern const int Horizon_SCAN = 1800;

// 3. 修改垂直角度分辨率 (ang_res_y) 和 底部角度 (ang_bottom)
// 这是一个极其关键的步骤，必须查阅 Hesai 手册！
// 公式：ang_res_y = (垂直FOV范围) / (N_SCAN - 1)
// 示例 (Pandar XT-32): FOV是31度 (-16 ~ +15)
extern const float ang_res_y = 31.0 / 31.0; // 约等于 1.0
extern const float ang_bottom = 16.0;       // 底部角度的绝对值(如果最下面是-16度，这里填16.0)
extern const int groundScanInd = 20;        // 地面点检测阈值，线束越多，这个值可能需要适当调大

我的hesai雷达是128线的，一般来说需要降采样，所以我直接把Hesai的数据格式转换成 Velodyne 的格式了。

3.2 话题订阅修改

在 imageProjection.cpp 中，修改订阅的话题名称：

// 原始代码
// subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &ImageProjection::cloudHandler, this);

// 修改为 Docker 抛出 Hesai 转换成 Velodyne 的格式话题
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/points_raw", 1, &ImageProjection::cloudHandler, this);

4. 编译与验证

4.1 重新编译 (宿主机)

cd ~/catkin_ws  # 进入工作空间
catkin_make -j4 # 重新编译
source devel/setup.bash

4.2 联合调试步骤

1. Docker 端：启动雷达驱动。

2. 宿主机端：启动 LeGO-LOAM。

3. Rviz 验证 (关键)：

• 将 Fixed Frame 设置为 base_link。

• 添加 PointCloud2，选择 /laser_cloud_surround。

• 检查 TF 树：运行 rosrun rqt_tf_tree rqt_tf_tree。

• 预期结果：应该能看到一条完整的链：map -> camera_init -> ... -> base_link -> hesai_lidar。

下图分别是没连接成功的图和连接好的图：

5、LeGO-LOAM 建图后处理：地图保存、可视化与轨迹提取

1. 保存 3D 点云地图 (.pcd)

LeGO-LOAM 的全局地图话题通常是 /laser_cloud_surround。我们可以使用 pcl_ros 包来接收这个话题并保存文件。

在建图结束（跑完数据包或绕完一圈）且 ROS 节点尚未关闭 时，在宿主机打开一个新的终端，执行以下命令：

# 创建一个存放地图的文件夹
mkdir -p ~/map_output
cd ~/map_output

# 使用 pcl_ros 监听话题并保存
# input:= 后面跟的是 LeGO-LOAM 的全局地图话题
# _prefix:= 是保存文件的前缀
rosrun pcl_ros pointcloud_to_pcd input:=/laser_cloud_surround _prefix:=scans_

操作技巧：

1. 输入命令后，您会看到终端不断打印 Data saved to ...。

2. 因为 /laser_cloud_surround 是低频更新的（或者一直在发布），您只需要等它保存了一帧完整的地图后（通常几秒钟），即可按 Ctrl+C 停止。

3. 查看文件夹，您会发现生成了 scans_xxxxx.pcd 文件，文件最大的那个通常就是最终的完整地图。

当然也可以通过 Rosbag 录制来保存，但我觉得太麻烦了。

2. 查看与编辑地图

保存好的 .pcd 文件无法直接用图片浏览器打开，需要专业工具。

工具一：pcl_viewer (轻量级预览)

如果安装了 PCL 库（ROS 自带），可以直接在终端查看：

pcl_viewer ~/map_output/scans_xxxxx.pcd

• 操作：鼠标左键旋转，滚轮缩放，Shift+左键拖动。

• 常用快捷键：按 u 切换颜色模式（根据强度或高度），按 r 重置视角。

工具二：CloudCompare (科研/工程推荐)

这是处理点云的神器（Windows/Ubuntu 均有）。对于写论文或做标注来说，它比 pcl_viewer 强大太多。

功能：

• 裁剪：可以手动删在这个地图里出现的动态物体（如行人、路过的车）。

• 格式转换：可以把 .pcd 转成 .ply 或 .las。

• 渲染：可以生成非常漂亮的鸟瞰图用于论文配图。

3. 提取轨迹用于精度评估

作为研究人员，可能不仅要图，还要评估 SLAM 的精度（ATE/RPE）。LeGO-LOAM 在运行时通常会把轨迹数据输出到终端或特定目录下。

如果想用 evo 工具评估轨迹，建议修改 mapOptmization.cpp，在保存 keyframe pose 的地方添加一段代码，将其保存为 TUM 或 KITTI 格式的 .txt 文件。

或者，直接录制 /aft_mapped_to_init 话题：

rosbag record -O trajectory.bag /aft_mapped_to_init

然后使用 evo 工具直接读取 bag 文件进行画图：

evo_traj bag trajectory.bag /aft_mapped_to_init --plot

对于精度评估没写全，下次做实验的时候再仔细讲解一下吧。