透过现象看本质：以fast_lio架构的整套算法的局部避障改为TEB算法为例深度探讨——如何成为一个合格的算法架构师？
本文回答了一个工程如果要修改代码算法，应该如何去着手，怎么实现？原先的逻辑和数据流是怎样的？
作为前人留下的工程，我们在修改它时，最重要的原则就是“黑盒替换（接口对齐）”
以下以本作者在从第一次拿到基于fast_lio框架的导航代码到修改代码采用TEB算法的全过程为例。
第一部分：拆解原系统的框架和数据流
原系统的数据流向图（核心主线）：
- 【感知端】3D雷达 -> 障碍物提取： livox 雷达输出点云 -> dual_radar_extractor（利用 Patchwork++ 算法把地面、楼梯滤除） -> 输出纯障碍物点云（估计话题名叫 /obstacle_cloud 或类似名称）。知识点：行业经验储备】 当 SLAM 工程师看到 ground_seg (Ground Segmentation，地面分割) 和 patchworkpp 这两个词放在一起时，DNA 就动了。 Patchwork++ 是一篇著名的开源论文算法，专门解决复杂 3D 地形（包括斜坡、楼梯、坑洼）的地面分割问题。它输出两个结果：
- ground_cloud（可通行的地面/楼梯点云）
- non_ground_cloud / obstacle_cloud（墙壁、行人等纯障碍物点云）
- 【局部规划端】也就是调整局部规划参数的 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++ 代码里，开发者通常会写死一个内部名字（比如 from="/cmd_vel"）。但为了让这个节点能适配不同的机器人，会在 launch 文件里用 <remap> 把它映射到系统真实的话题名（to="$(arg cmd_vel_topic)"）。 因为这里的 default 是 /cmd_vel，所以可以100%确信，底层底盘（go2_ros_control）接收速度的话题就是 /cmd_vel。
【规划端】全局地图 -> 3D全局路径： global_path_planner 在你构建好的拓扑地图和空心 3D PCD 中寻路 -> 输出带有高度的 3D 全局路径（话题：/global_path）。确认接口话题：打开 pure_pursuit_params.yaml：
global_path_topic: "/global_path" # 全局路径话题【知识点：发布/订阅契约】 局部规划器（Pure Pursuit）是“消费者”，它订阅了 /global_path。在 ROS 的世界里，有消费者就必然有“生产者”。谁是生产者？必然是全局规划器（Global Planner）。所以，全局规划器一定在往 /global_path 这个话题发数据。确认全局规划的逻辑（拓扑+PCD）： 回头看目录树：
./global_planner/topology_rviz_plugin/map: connected_topology_map.txt key_nodes_map.txt ./global_planner/pcd_map/pcd: aft_pgo_free_space.pcd我看到了拓扑节点文本（key_nodes_map.txt），看到了 PGO（位姿图优化）之后提取的空心自由空间点云（aft_pgo_free_space.pcd）。 这就证明了：你的全局规划器在工作时，是先读取这些预先建好的 3D 地图，用图搜索算法（A* 或 Dijkstra）在拓扑点之间连线，最终生成一条具有 (x, y, z) 三维坐标的路径，发到了 /global_path 上。
原系统痛点： 为什么我觉得它导航得很烂？因为上一个工程师用传统 A* 去做局部动态避障，A* 是没有“时间”和“机器人运动学（加速度、最大角速度）”概念的，算出来的路机器人走起来很生硬；另外他还加了人工势场法（Potential Field），这很容易让狗子在狭窄走廊里陷入“死锁（卡在原地左右乱晃）”。
enable_local_replanning: true # 启用了局部重规划 dijkstra_obstacle_threshold: 10 # 竟然在纯追踪里套了一个 Dijkstra 算法！ potential_field: # 甚至还加了“人工势场法”！ attractive_gain: 8.0 # 吸引力 repulsive_gain: 1.0 # 排斥力第二部分：讲解工程改造的逻辑方法论
当我们要将原先的 pure_pursuit 替换为 TEB 时，标准的工作流是这样的：
步骤1：找到要替换的“黑盒” 在这个项目里，“黑盒”就是 pure_pursuit_local_planner_node。
步骤2：明确“黑盒”的输入和输出接口（Interface）
- 需要什么（输入）：
- 机器人当前位置（Odometry / TF树）
- 全局路径 (nav_msgs/Path，话题 /global_path)
- 障碍物信息（局部代价地图 Local Costmap）
- 产出什么（输出）：
底盘速度指令 (geometry_msgs/Twist，话题 /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++ 代码里，开发者通常会写死一个内部名字（比如 from="/cmd_vel"）。但为了让这个节点能适配不同的机器人，会在 launch 文件里用 <remap> 把它映射到系统真实的话题名（to="$(arg cmd_vel_topic)"）。 因为这里的 default 是 /cmd_vel，所以可以100%确信，底层底盘（go2_ros_control）接收速度的话题就是 /cmd_vel。
第三部分：给出TEB 最终版代码。
步骤 1：创建 TEB 功能包
在你的终端里运行（如果已经建了就跳过）：
cd ~/ws_nav/src/go2_real/navigation_3d/local_planner catkin_create_pkg teb_local_tracker roscpp tf2 tf2_ros costmap_2d teb_local_planner nav_msgs geometry_msgs cd teb_local_tracker mkdir config launch步骤 2：写入最终版 YAML 配置文件
新建 config/teb_costmap_params.yaml，填入以下内容（注意看填入的雷达话题名）：
# ================= 局部代价地图配置 ================= local_costmap: global_frame: map robot_base_frame: base_link update_frequency: 10.0 publish_frequency: 10.0 transform_tolerance: 0.5 rolling_window: true width: 5.0 height: 5.0 resolution: 0.05 plugins: - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"} obstacle_layer: observation_sources: point_cloud_sensor point_cloud_sensor: { sensor_frame: mid360_link, data_type: PointCloud2, # 【核心对接点】：只接收你查出来的纯障碍物点云！楼梯对TEB隐身！ topic: /dual_radar_traversable_extractor/obstacle_cloud, marking: true, clearing: true, min_obstacle_height: 0.1, max_obstacle_height: 1.5 } inflation_layer: inflation_radius: 0.35 # 狗的膨胀安全半径 cost_scaling_factor: 5.0 # ================= TEB 局部规划器配置 ================= TebLocalPlannerROS: odom_topic: /Odometry map_frame: map # 速度限制 (继承自你原先的 yaml) max_vel_x: 0.6 max_vel_x_backwards: 0.2 max_vel_theta: 0.9 acc_lim_x: 0.5 acc_lim_theta: 0.5 min_obstacle_dist: 0.35 include_costmap_obstacles: True # 路径跟随 (紧跟 3D 全局路径) global_plan_viapoint_sep: 0.5 weight_viapoint: 10.0 weight_obstacle: 50.0 # 运动学 (四足差速模型) weight_kinematics_nh: 1000.0 weight_kinematics_forward_drive: 10.0步骤 3：写入核心 C++ 节点
在 src/teb_tracker_node.cpp 中填入这个极简的包装器（我已优化过）：
#include <ros/ros.h> #include <tf2_ros/transform_listener.h> #include <costmap_2d/costmap_2d_ros.h> #include <teb_local_planner/teb_local_planner_ros.h> #include <nav_msgs/Path.h> #include <geometry_msgs/Twist.h> class TebTracker { public: TebTracker(tf2_ros::Buffer& tf) : costmap_ros_("local_costmap", tf) { teb_planner_.initialize("TebLocalPlannerROS", &tf, &costmap_ros_); // 【核心对接点】：订阅你扒出来的全局路径 path_sub_ = nh_.subscribe("/global_path", 1, &TebTracker::pathCallback, this); // 【核心对接点】：发布你扒出来的底盘控制指令 vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1); timer_ = nh_.createTimer(ros::Duration(0.1), &TebTracker::controlLoop, this); ROS_INFO("TEB Tracker Initialized! Ready to dodge obstacles and climb stairs!"); } private: void pathCallback(const nav_msgs::Path::ConstPtr& msg) { current_path_ = *msg; std::vector<geometry_msgs::PoseStamped> plan = msg->poses; teb_planner_.setPlan(plan); } void controlLoop(const ros::TimerEvent&) { if (current_path_.poses.empty()) return; geometry_msgs::Twist cmd_vel; if (teb_planner_.computeVelocityCommands(cmd_vel)) { vel_pub_.publish(cmd_vel); } else { ROS_WARN_THROTTLE(1.0, "TEB Planner Blocked! Outputting zero velocity."); cmd_vel.linear.x = 0.0; cmd_vel.angular.z = 0.0; vel_pub_.publish(cmd_vel); } } ros::NodeHandle nh_; ros::Subscriber path_sub_; ros::Publisher vel_pub_; ros::Timer timer_; costmap_2d::Costmap2DROS costmap_ros_; teb_local_planner::TebLocalPlannerROS teb_planner_; nav_msgs::Path current_path_; }; int main(int argc, char** argv) { ros::init(argc, argv, "teb_tracker_node"); tf2_ros::Buffer buffer(ros::Duration(10)); tf2_ros::TransformListener tf(buffer); TebTracker tracker(buffer); ros::spin(); return 0; }步骤 4：写入 Launch 文件
新建 launch/teb_tracker.launch：
<?xml version="1.0"?> <launch> <node pkg="teb_local_tracker" type="teb_tracker_node" name="teb_tracker_node" output="screen"> <rosparam file="$(find teb_local_tracker)/config/teb_costmap_params.yaml" command="load" /> </node> </launch>步骤 5：修改 CMakeLists.txt
打开 CMakeLists.txt，在最底部加上这两行，告诉系统去编译你的 C++ 文件：
add_executable(teb_tracker_node src/teb_tracker_node.cpp) target_link_libraries(teb_tracker_node ${catkin_LIBRARIES})编译与测试（迎来胜利的曙光）
- 终极验证： 跑起 3_nav_start.sh！ 如果在终端里看到了绿色字体：TEB Tracker Initialized! Ready to dodge obstacles and climb stairs! 那就大功告成了！你可以给狗子发一个包含楼梯的导航目标点，看看它是不是能平滑地避开障碍物，并且勇敢地踏上楼梯。
替换启动脚本： 打开最开始的那个大总管脚本 3_nav_start.sh，把原本启动 pure_pursuit 和 path_smoother 的行注释掉，加上这行：
# ----------------- 注释掉原有的 pure pursuit ----------------- # launch_bg "pure pursuit" roslaunch pure_pursuit_local_planner pure_pursuit_local_planner.launch # sleep 1 # ----------------- 保留并确保启动雷达可通行区域提取 ----------------- # 这个必须启动，因为它的 patchworkpp 会把楼梯识别为地面，把人识别为障碍物 launch_bg "dual radar extractor" roslaunch dual_radar_traversable_extractor dual_radar_extractor.launch sleep 3 # ----------------- 新增启动 TEB Tracker ----------------- launch_bg "TEB local tracker" roslaunch teb_local_tracker teb_tracker.launch sleep 1编译代码： 回到工作空间根目录，执行编译：
cd ~/ws_nav catkin_make source devel/setup.bash注意：如果提示找不到 teb_local_planner，记得先运行 sudo apt-get install ros-noetic-teb-local-planner 安装。
总结：工程修改的方法论：
修改一个大型的机器人工程，绝对不是拿到代码就开始改 C++。标准流程是：
- 看文件结构和命名（找轮子）：看到 patchworkpp 就知道地面分割不用愁了；看到 topology 就知道全局规划是 3D 图搜索。
- 看 Launch 和 Yaml（摸清数据流）：从 remap 找话题名字，从 param 找上一个工程师的算法逻辑（比如发现了坑人的势场法）。
- 确定边界，黑盒替换：确认老规划器吃的是 Path，吐的是 Twist(cmd_vel)。我们写个新的 TEB 节点，同样吃 Path 吐 Twist。
- 最后才是敲代码：把新的 C++ 节点写出来，放到系统里。
