透过现象看本质：以fast_lio架构的整套算法的局部避障改为TEB算法为例深度探讨——如何成为一个合格的算法架构师？

本文回答了一个工程如果要修改代码算法，应该如何去着手，怎么实现？原先的逻辑和数据流是怎样的？

作为前人留下的工程，我们在修改它时，最重要的原则就是"黑盒替换（接口对齐）"

以下以本作者在从第一次拿到基于fast_lio框架的导航代码到修改代码采用TEB算法的全过程为例。

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

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

【感知端】3D雷达 -> 障碍物提取： livox 雷达输出点云 -> dual_radar_extractor（利用 Patchwork++ 算法把地面、楼梯滤除） -> 输出纯障碍物点云（估计话题名叫 /obstacle_cloud 或类似名称）。

知识点：行业经验储备】 当 SLAM 工程师看到 ground_seg (Ground Segmentation，地面分割) 和 patchworkpp 这两个词放在一起时，DNA 就动了。 Patchwork++ 是一篇著名的开源论文算法，专门解决复杂 3D 地形（包括斜坡、楼梯、坑洼）的地面分割问题。它输出两个结果：
1. ground_cloud（可通行的地面/楼梯点云）
2. non_ground_cloud / obstacle_cloud（墙壁、行人等纯障碍物点云）
【规划端】全局地图 -> 3D全局路径： global_path_planner 在你构建好的拓扑地图和空心 3D PCD 中寻路 -> 输出带有高度的 3D 全局路径（话题：/global_path）。

确认接口话题：打开 pure_pursuit_params.yaml：
TypeScript 复制代码
```
global_path_topic: "/global_path"  # 全局路径话题
```
【知识点：发布/订阅契约】 局部规划器（Pure Pursuit）是"消费者"，它订阅了 /global_path。在 ROS 的世界里，有消费者就必然有"生产者"。谁是生产者？必然是全局规划器（Global Planner）。所以，全局规划器一定在往 /global_path 这个话题发数据。

确认全局规划的逻辑（拓扑+PCD）：回头看目录树：
bash 复制代码
```
./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 上。
【局部规划端】也就是调整局部规划参数的 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
  复制代码
```
    
    <arg name="cmd_vel_topic" default="/cmd_vel" />
    
    <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。

原系统痛点： 为什么我觉得它导航得很烂？因为上一个工程师用传统 A* 去做局部动态避障，A* 是没有"时间"和"机器人运动学（加速度、最大角速度）"概念的，算出来的路机器人走起来很生硬；另外他还加了人工势场法（Potential Field），这很容易让狗子在狭窄走廊里陷入"死锁（卡在原地左右乱晃）"。

TypeScript 复制代码

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）

需要什么（输入）：
1. 机器人当前位置（Odometry / TF树）
2. 全局路径 (nav_msgs/Path，话题 /global_path)
3. 障碍物信息（局部代价地图 Local Costmap）
产出什么（输出）：
1. 底盘速度指令 (geometry_msgs/Twist，话题 /cmd_vel)
  
  打开 pure_pursuit_params.yaml：
  TypeScript 复制代码
```
cmd_vel_topic: "/cmd_vel"          # 控制指令话题
```
  打开 pure_pursuit_local_planner.launch
  XML 复制代码
```
  
    <arg name="cmd_vel_topic" default="/cmd_vel" />
    
    <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 功能包

在你的终端里运行（如果已经建了就跳过）：

bash 复制代码

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，填入以下内容（注意看填入的雷达话题名）：

XML 复制代码

# ================= 局部代价地图配置 =================
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 中填入这个极简的包装器（我已优化过）：

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 复制代码

<?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++ 文件：

html 复制代码

add_executable(teb_tracker_node src/teb_tracker_node.cpp)
target_link_libraries(teb_tracker_node ${catkin_LIBRARIES})

编译与测试（迎来胜利的曙光）

编译代码：回到工作空间根目录，执行编译：
bash 复制代码
```
cd ~/ws_nav
catkin_make
source devel/setup.bash
```
注意：如果提示找不到 teb_local_planner，记得先运行 sudo apt-get install ros-noetic-teb-local-planner 安装。

替换启动脚本：打开最开始的那个大总管脚本 3_nav_start.sh，把原本启动 pure_pursuit 和 path_smoother 的行注释掉，加上这行：

XML 复制代码

# ----------------- 注释掉原有的 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

终极验证：跑起 3_nav_start.sh！如果在终端里看到了绿色字体：TEB Tracker Initialized! Ready to dodge obstacles and climb stairs! 那就大功告成了！你可以给狗子发一个包含楼梯的导航目标点，看看它是不是能平滑地避开障碍物，并且勇敢地踏上楼梯。

总结：工程修改的方法论：

修改一个大型的机器人工程，绝对不是拿到代码就开始改 C++。标准流程是：

看文件结构和命名（找轮子）：看到 patchworkpp 就知道地面分割不用愁了；看到 topology 就知道全局规划是 3D 图搜索。
看 Launch 和 Yaml（摸清数据流）：从 remap 找话题名字，从 param 找上一个工程师的算法逻辑（比如发现了坑人的势场法）。
确定边界，黑盒替换：确认老规划器吃的是 Path，吐的是 Twist(cmd_vel)。我们写个新的 TEB 节点，同样吃 Path 吐 Twist。
最后才是敲代码：把新的 C++ 节点写出来，放到系统里。