基于 Fast_LIO 架构将局部规划从 Pure Pursuit 迁移至 TEB 算法的实践

工程算法修改方法论：以 Fast_LIO 导航系统为例

在工程实践中修改现有代码算法时，核心原则是'黑盒替换（接口对齐）'。以下以从基于 Fast_LIO 框架的导航代码修改为采用 TEB 算法的全过程为例。

第一部分：拆解原系统的框架和数据流

原系统的数据流向图（核心主线）

1. 感知端：3D 雷达 -> 障碍物提取

   • Livox 雷达输出点云 -> dual_radar_extractor（利用 Patchwork++ 算法把地面、楼梯滤除） -> 输出纯障碍物点云（估计话题名叫 /obstacle_cloud 或类似名称）。
   • 知识点：行业经验储备：当 SLAM 工程师看到 ground_seg (Ground Segmentation，地面分割) 和 patchworkpp 这两个词放在一起时，容易联想到其功能。Patchwork++ 是一篇著名的开源论文算法，专门解决复杂 3D 地形（包括斜坡、楼梯、坑洼）的地面分割问题。它输出两个结果：
     1. ground_cloud（可通行的地面/楼梯点云）
     2. non_ground_cloud / obstacle_cloud（墙壁、行人等纯障碍物点云）

2. 局部规划端：调整局部规划参数的 pure_pursuit 节点（集成方案）

   • 它接收 /global_path 和 /scan（或局部代价地图）。
   • 工程师在 pure_pursuit 里面塞入了一个小型的 A* 和 Dijkstra 算法（从 yaml 的 enable_local_replanning: true 可以看出）。
   • 当遇到障碍物时，它先用小 A* 绕开，然后再用自带的 path_smoother_node 把这段绕开的路径变平滑，最后再用纯追踪算法（Pure Pursuit）去算左右轮该转多快。
   • 最终输出速度指令（话题：/cmd_vel）。
   • 打开 pure_pursuit_params.yaml：

     cmd_vel_topic: "/cmd_vel"  # 控制指令话题
     

   • 打开 pure_pursuit_local_planner.launch：

     <!-- Pure Pursuit 接收的接口 -->
     <arg name="cmd_vel_topic" default="/cmd_vel" />
     <!-- Topic remapping -->
     <remap from="/cmd_vel" to="$(arg cmd_vel_topic)" />
     

   • 知识点：ROS Remap 机制：在 ROS C++ 代码里，开发者通常会写死一个内部名字（比如 ）。但为了让这个节点能适配不同的机器人，会在 launch 文件里用 把它映射到系统真实的话题名（）。因为这里的 default 是 ，所以可以确信，底层底盘（go2_ros_control）接收速度的话题就是 。