1 概述

1.1 Progress Checker

Progress_checker: 判断机器人在一段时间内是否真的在朝目标前进.

插件名 plugin 类名 进度判定依据 主要参数（单位） 典型场景 备注
SimpleProgressChecker nav2_controller::SimpleProgressChecker 位移是否在给定时间内达到最小半径 required_movement_radius（m）、movement_time_allowance（s） 直线/曲线前进为主，原地转向不重要的移动机器人 实现简单、开销小；原地旋转不计为“进步”
PoseProgressChecker nav2_controller::PoseProgressChecker 位移或转角是否在给定时间内达到阈值 required_movement_radius（m）、required_movement_angle（rad）、movement_time_allowance（s） 频繁原地转向（窄通道、角落对齐）也应被视为“有进步” 能避免机器人原地找路时被误判为卡住

1.2 Goal Checker

goal_checker: 判断机器人是否到达目标.

插件名 plugin 类名 判定条件 主要参数（单位） 典型场景 备注
SimpleGoalChecker nav2_controller::SimpleGoalChecker 当前位置到目标的平移距离 ≤ 阈值 且 朝向差 ≤ 阈值 xy_goal_tolerance（m）、yaw_goal_tolerance（rad）、stateful（是否保留状态） 通用场景，需要位置与朝向都满足 最常用的基础型目标检测器
StoppedGoalChecker nav2_controller::StoppedGoalChecker 同 SimpleGoalChecker 条件 且 速度低于阈值 xy_goal_tolerance（m）、yaw_goal_tolerance（rad）、trans_stopped_velocity（m/s）、rot_stopped_velocity（rad/s）、stateful 要求机器人到达目标后必须停稳才算完成 用于避免“到点不停”问题
PositionGoalChecker nav2_controller::PositionGoalChecker 当前位置到目标的平移距离 ≤ 阈值，不要求朝向收敛 xy_goal_tolerance（m）、stateful 机械臂基座、搬运机器人等对朝向不敏感的任务 可缩短路径时间，但可能朝向不一致

1.3 Controller

Nav2 的局部控制器插件清单(也叫”Local Planner”).

它们在 controller_server 中运行：读取全局路径和实时代价地图，按照各自的算法在高频率下计算速度指令 cmd_vel，让机器人沿路径安全、平滑地前进.

你可以在 controller_server.ros__parameters.controller_plugins 下同时加载多个控制器，并通过行为树(ControllerSelector)或在 Action 目标里设置 controller_id 来动态切换.

控制器 plugin 类名（Jazzy） 作者 简介 适配底盘
DWB Controller dwb_core::DWBLocalPlanner David Lu!! 动态窗口法（DWA）的高度可配置实现，支持多种代价插件 差速、全向、足式
TEB Controller teb_local_planner::TebLocalPlannerROS Christoph Rösmann 近似 MPC 的时弹性带方法，优化时间+空间，适合多约束场景 Ackermann、足式、全向、差速
Regulated Pure Pursuit nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController Steve Macenski 纯追踪改进版，自动调节速度/曲率，工业移动机器人常用 Ackermann、足式、差速
MPPI Controller nav2_mppi_controller::MPPIController Steve Macenski, Aleksei Budyakov 预测控制（MPPI），采样优化+可插拔 critic，通用性强 差速、全向、Ackermann
Rotation Shim Controller nav2_rotation_shim_controller::RotationShimController Steve Macenski “垫片”控制器：先原地对准路径方向，再交给主控制器跟踪 差速、全向（能原地旋转的底盘）
Graceful Controller nav2_graceful_controller::GracefulController Alberto Tudela 基于位姿跟随的控制律，轨迹平滑、收敛稳定 差速、全向、足式
Vector Pursuit Controller nav2_vector_pursuit_controller::VectorPursuitController Black Coffee Robotics 向量追踪算法，适合高速且要求高精度的路径跟踪 差速、Ackermann、足式

2 参数解析

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
controller_server:
  ros__parameters:
    controller_frequency: 20.0 # 控制循环频率
    costmap_update_timeout: 0.30 # 代价地图更新的最大超时时间

    # 速度死区, 小于该值的速度会被当作 0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    
    failure_tolerance: 0.3 # 插件抛出异常的时间, 超出则认为失败. -1.0 表示无限长, 0 表示禁用.
    
    # 进度检测器插件名称列表
    progress_checker_plugins: ["progress_checker"]

    # 目标检测器插件
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    
    # 局部规划器插件(Local Planner)
    controller_plugins: ["FollowPath"]
    
    use_realtime_priority: false # 优先级分配

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.15
      yaw_goal_tolerance: 0.15
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      ax_max: 3.0
      ax_min: -3.0
      ay_max: 3.0
      az_max: 3.5
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 0.2
      vx_min: -0.2
      vy_max: 0.5
      wz_max: 1.9
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: true
      regenerate_noises: true
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstraints:
        min_turning_r: 0.2
      critics: [
        "ConstraintCritic", "CostCritic", "GoalCritic",
        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
        "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        near_collision_cost: 253
        critical_cost: 300.0
        consider_footprint: false
        collision_cost: 1000000.0
        near_goal_distance: 1.0
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 0
      # TwirlingCritic:
      #   enabled: true
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.0

3 自定义局部规划器

包结构.

1
2
3
4
5
6
nav2_pure_pursuit_controller/
├── CMakeLists.txt
├── package.xml
├── pure_pursuit_controller_plugin.xml
├── include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp
└── src/pure_pursuit_controller.cpp

头文件.

方法名 触发时机 / 生命周期 作用 关键入参 / 返回 必须重写
configure() controller_server 进入 on_configure 声明参数、初始化成员（拿到 TF、Costmap、日志/时钟等） 入参：parent(WeakPtr)、name、tf、costmap_ros；返回：void 是
activate() 进入 on_activate 启动发布器/定时器、切换到活跃态 入参：无；返回：void 是
deactivate() 进入 on_deactivate 停止发布器/定时器、退出活跃态 入参：无；返回：void 是
cleanup() 进入 on_cleanup 释放 configure() 创建的资源、指针置空 入参：无；返回：void 是
setPlan() 全局路径更新时 处理并缓存最新的全局路径（常见：变换到机器人坐标系） 入参：nav_msgs::msg::Path；返回：void 是
computeVelocityCommands() 控制器需要新速度指令时（每个控制周期） 计算并返回速度指令以跟随路径 入参：pose(当前位姿)、velocity(当前速度)、goal_checker*；返回：geometry_msgs::msg::TwistStamped 是
cancel() 收到取消请求时 可选的“优雅停止”流程；不实现则立即停 入参：无；返回：实现体裁（常见 bool） 可选
setSpeedLimit() 需要限速时 以绝对值(m/s)或百分比限制最大线速度（角速度通常按比例限） 入参：speed_limit、percentage；返回：void 是 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
#pragma once
#include <string>
#include <vector>
#include <memory>
#include <functional>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_core/controller.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/buffer.h"

namespace nav2_pure_pursuit_controller {

class PurePursuitController : public nav2_core::Controller
{
public:
  PurePursuitController() = default;
  ~PurePursuitController() override = default;

  // 生命周期
  void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
                 std::string name,
                 std::shared_ptr<tf2_ros::Buffer> tf,
                 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

  void cleanup() override;
  void activate() override;
  void deactivate() override;

  // 关键接口
  void setPlan(const nav_msgs::msg::Path & path) override;

  geometry_msgs::msg::TwistStamped computeVelocityCommands(
      const geometry_msgs::msg::PoseStamped & pose,
      const geometry_msgs::msg::Twist & velocity,
      nav2_core::GoalChecker * goal_checker) override;

  void setSpeedLimit(const double & speed_limit, const bool & percentage) override;

  // 可选：取消（不实现也行）
  bool cancel() override { return true; }

private:
  nav_msgs::msg::Path transformGlobalPlan(const nav_msgs::msg::Path & path);

  // 基础资源
  rclcpp_lifecycle::LifecycleNode::WeakPtr weak_node_;
  rclcpp::Logger logger_{rclcpp::get_logger("PurePursuitController")};
  rclcpp::Clock::SharedPtr clock_;
  std::shared_ptr<tf2_ros::Buffer> tf_;
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
  std::string plugin_name_;
  std::string base_frame_;
  rclcpp::Duration transform_tolerance_{0, 0};

  // 计划与参数
  nav_msgs::msg::Path global_plan_robot_frame_;
  double desired_linear_vel_{0.2};  // m/s
  double lookahead_dist_{0.4};      // m
  double max_angular_vel_{1.0};     // rad/s

  // 速度限制（来自 setSpeedLimit）
  double linear_speed_limit_abs_{std::numeric_limits<double>::infinity()};
  double linear_speed_limit_scale_{1.0};  // percentage/100
  bool use_percentage_limit_{false};
};

} // namespace nav2_pure_pursuit_controller

源代码.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
#include <algorithm>
#include <cmath>
#include "nav2_pure_pursuit_controller/pure_pursuit_controller.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

namespace nav2_pure_pursuit_controller {

static inline void declare_parameter_if_not_declared(
  const rclcpp::Node::SharedPtr & node, const std::string & name,
  const rclcpp::ParameterValue & default_value)
{
  if (!node->has_parameter(name)) {
    node->declare_parameter(name, default_value);
  }
}

void PurePursuitController::configure(
  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
  std::string name,
  std::shared_ptr<tf2_ros::Buffer> tf,
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
  weak_node_ = parent;
  auto node = weak_node_.lock();
  tf_ = tf;
  costmap_ros_ = costmap_ros;
  plugin_name_ = name;
  logger_ = node->get_logger();
  clock_ = node->get_clock();
  base_frame_ = costmap_ros_->getBaseFrameID();

  declare_parameter_if_not_declared(node, plugin_name_ + ".desired_linear_vel", 0.2);
  declare_parameter_if_not_declared(node, plugin_name_ + ".lookahead_dist", 0.4);
  declare_parameter_if_not_declared(node, plugin_name_ + ".max_angular_vel", 1.0);
  declare_parameter_if_not_declared(node, plugin_name_ + ".transform_tolerance", 0.1);

  node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
  node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_);
  node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_);
  double tol_sec{0.1};
  node->get_parameter(plugin_name_ + ".transform_tolerance", tol_sec);
  transform_tolerance_ = rclcpp::Duration::from_seconds(tol_sec);

  RCLCPP_INFO(logger_, "[%s] cfg: v=%.3f m/s, Ld=%.3f m, wz_max=%.3f rad/s",
              plugin_name_.c_str(), desired_linear_vel_, lookahead_dist_, max_angular_vel_);
}

void PurePursuitController::activate() {}
void PurePursuitController::deactivate() {}
void PurePursuitController::cleanup() { global_plan_robot_frame_.poses.clear(); }

void PurePursuitController::setPlan(const nav_msgs::msg::Path & path)
{
  global_plan_robot_frame_ = transformGlobalPlan(path);
}

nav_msgs::msg::Path PurePursuitController::transformGlobalPlan(const nav_msgs::msg::Path & path)
{
  nav_msgs::msg::Path out;
  if (path.poses.empty()) return out;

  out.header.stamp = clock_->now();
  out.header.frame_id = base_frame_;

  const std::string & from_frame = path.header.frame_id;
  if (from_frame == base_frame_) {
    out.poses = path.poses;
    return out;
  }

  geometry_msgs::msg::TransformStamped tf_stamped;
  try {
    tf_stamped = tf_->lookupTransform(base_frame_, from_frame, tf2::TimePointZero,
                                      tf2::durationFromSec(transform_tolerance_.seconds()));
  } catch (const tf2::TransformException & ex) {
    RCLCPP_WARN(logger_, "TF transform failed: %s", ex.what());
    return out;
  }

  out.poses.reserve(path.poses.size());
  for (const auto & p : path.poses) {
    geometry_msgs::msg::PoseStamped pr = p;
    tf2::doTransform(pr, pr, tf_stamped);
    out.poses.push_back(pr);
  }
  return out;
}

void PurePursuitController::setSpeedLimit(const double & speed_limit, const bool & percentage)
{
  use_percentage_limit_ = percentage;
  if (percentage) {
    linear_speed_limit_scale_ = std::max(0.0, speed_limit) / 100.0; // 0~1
  } else {
    linear_speed_limit_abs_ = (speed_limit <= 0.0) ?
      std::numeric_limits<double>::infinity() : speed_limit;
  }
}

geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::Twist & /*velocity*/,
    nav2_core::GoalChecker * /*goal_checker*/)
{
  geometry_msgs::msg::TwistStamped cmd;
  cmd.header.frame_id = pose.header.frame_id;
  cmd.header.stamp = clock_->now();

  if (global_plan_robot_frame_.poses.empty()) {
    RCLCPP_WARN_THROTTLE(logger_, *clock_, 2000, "[%s] Empty plan.", plugin_name_.c_str());
    return cmd; // 全零 -> 停止
  }

  // 选择“离机器人 ≥ lookahead_dist_”的第一个目标点（已在机器人坐标系）
  auto it = std::find_if(global_plan_robot_frame_.poses.begin(),
                         global_plan_robot_frame_.poses.end(),
                         [&](const auto & ps){
                           return std::hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist_;
                         });

  geometry_msgs::msg::Pose target = (it == global_plan_robot_frame_.poses.end())
      ? global_plan_robot_frame_.poses.back().pose
      : it->pose;

  // 纯追踪：曲率 = 2*y / (x^2 + y^2)
  const double x = target.position.x;
  const double y = target.position.y;

  double v = desired_linear_vel_;
  if (use_percentage_limit_) v = std::min(v, desired_linear_vel_ * linear_speed_limit_scale_);
  else                       v = std::min(v, linear_speed_limit_abs_);

  double w = 0.0;
  if (x > 0.0) {
    const double denom = (x * x + y * y);
    const double curvature = (denom > 1e-6) ? (2.0 * y / denom) : 0.0;
    w = v * curvature;
  } else {
    v = 0.0;                 // 目标在身后：原地转向
    w = (y >= 0.0) ?  max_angular_vel_ : -max_angular_vel_;
  }

  // 角速度限幅
  if (w >  max_angular_vel_) w =  max_angular_vel_;
  if (w < -max_angular_vel_) w = -max_angular_vel_;

  cmd.twist.linear.x  = v;
  cmd.twist.angular.z = w;
  return cmd;
}

} // namespace nav2_pure_pursuit_controller

// 导出为 Controller 插件
PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller)

插件描述.

1
2
3
4
5
6
<library path="nav2_pure_pursuit_controller">
  <class type="nav2_pure_pursuit_controller::PurePursuitController"
         base_class_type="nav2_core::Controller">
    <description>Pure pursuit controller (Jazzy)</description>
  </class>
</library>

package.xml 关键片段.

1
2
3
4
<export>
  <build_type>ament_cmake</build_type>
  <nav2_core plugin="${prefix}/global_planner_plugin.xml" />
</export>

在 yaml 文件加入自己的局部规划器.

1
2
3
4
5
6
7
8
9
10
11
controller_server:
  ros__parameters:
    controller_frequency: 20.0
    controller_plugins: ["FollowPath"]

    FollowPath:
      plugin: "nav2_pure_pursuit_controller::PurePursuitController"
      desired_linear_vel: 0.25
      lookahead_dist: 0.5
      max_angular_vel: 1.0
      transform_tolerance: 0.1

4 References