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

原系统痛点

为什么觉得导航效果不佳？因为上一个工程师用传统 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）

• 需要什么（输入）：
  1. 机器人当前位置（Odometry / TF 树）
  2. 全局路径 (nav_msgs/Path，话题 /global_path)
  3. 障碍物信息（局部代价地图 Local Costmap）
• 产出什么（输出）：
  • 底盘速度指令 (geometry_msgs/Twist，话题 /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
      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
  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
  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})

编译与测试

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

2. 替换启动脚本：打开最开始的那个大总管脚本 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

3. 编译代码：回到工作空间根目录，执行编译：

cd ~/ws_nav
catkin_make
source devel/setup.bash

注意：如果提示找不到 teb_local_planner，记得先运行 sudo apt-get install ros-noetic-teb-local-planner 安装。

总结：工程修改的方法论

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

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