navigation2-humble从零带读笔记第一篇：nav2_core

navigation2-humble从零带读笔记第一篇：nav2_core

---

免责声明：本文内容为笔者从零学习 Nav2 的学习笔记，为结合官方注释、个人理解及 AI 辅助解析整理而成。若存在解读偏差，欢迎大家指正，我会及时修正完善。

nav2_core 的本质

nav2_core 是纯抽象接口包 ------只有头文件，没有任何实现代码。

每个类的核心业务方法均为纯虚函数（= 0），此外还有虚析构函数和 Ptr 类型别名(管理的共享指针)。

它定义了 Nav2 所有插件必须遵守的"规则"（纯虚基类）。

---

推荐阅读顺序

按导航流水线顺序阅读，每个接口对应一个功能环节。

---

第一步：基础（异常）

exceptions.hpp + route_exceptions.hpp

exceptions.hpp

只有一个 PlannerException，继承自 std::runtime_error，出错时抛出异常对象，用于 catch 捕获。整个是常规的 C++ 异常处理机制，知道即可。

try {
  // 路径规划相关代码
  throw nav2_core::PlannerException("路径规划失败");
} catch (const nav2_core::PlannerException & e) {
  // 将异常描述信息输出到标准错误流
  std::cerr << e.what() << std::endl;
}

个人理解： 如果只是打印、没有具体的出错后处理逻辑，调试时直接用 RCLCPP_ERROR 或 std::cout 会更方便，调完直接注释掉即可。PlannerException 的价值在于：当你的规划器插件被上层 server 调用时，server 会统一 catch 它并做失败处理，这才是它存在的意义。

route_exceptions.hpp

专门为路由规划（Route） 设计的异常体系，比 PlannerException 更细化，定义了一套继承层次：

RouteException（基类）
├── OperationFailed          // 操作执行失败
├── NoValidRouteCouldBeFound // 找不到有效路由
├── TimedOut                 // 规划超时
├── RouteTFError             // TF 坐标变换错误
├── NoValidGraph             // 图数据无效
├── IndeterminantNodesOnGraph // 图中存在不确定节点
└── InvalidEdgeScorerUse     // 边评分器使用错误

现阶段先知道有这个分层结构即可，后续读 nav2_route 包时会用到。

---

第二步：全局规划

global_planner.hpp

核心方法： createPlan(start, goal) → Path

代表"从 A 到 B 规划一条全局路径"，具体算法由插件实现（如 A*、Dijkstra 等）。

其他方法（后续所有接口都有这四个，之后略过不重复说）：

方法	作用
configure(parent, name, tf, costmap_ros)	初始化插件，注入依赖（节点、TF、代价地图）
activate()	激活插件及其执行线程
deactivate()	停用插件及其执行线程
cleanup()	关闭时释放资源

---

第三步：路径平滑（可选后处理）

smoother.hpp

核心方法： smooth(path, max_time) → bool

对全局规划出的路径做平滑处理。

• 参数一：nav_msgs::msg::Path 类型的全局路径（in-out 参数，直接在原路径上修改）
• 参数二：rclcpp::Duration 类型的最大处理时间，超时返回 false，表示平滑未完成
• 这一步是可选的，需要单独的路径优化算法支撑（如样条插值、梯度下降等）

---

第四步：进度检查

progress_checker.hpp

核心方法： check(current_pose) → bool

判断机器人是否在向目标正常前进（防卡死）。

内部通过将当前位姿 与上一时间步位姿比较，检测是否有移动。

• 返回 false → 判定卡住 → 上层 server 触发恢复分支：
  • 重新规划路径
  • 清除代价地图障碍层
  • 旋转或后退
  • 原地等待
  • 多次恢复失败 → 中止导航任务，报告失败

reset() ：重置检查状态，下次调用 check() 时以当前位姿作为新的基准点。

---

第五步：目标检查

goal_checker.hpp

核心方法： isGoalReached(query_pose, goal_pose, velocity) → bool

判断是否满足"到达目标"的条件（位置误差 + 速度约束）。

参数	含义
query_pose	机器人当前位姿
goal_pose	目标位姿
velocity	当前速度

本质上就是对"当前状态是否已到达目标"做一次直接判定。

在典型局部规划器实现中，通常的做法是在 setPlan() 里将到达标志位初始化为 false，然后每个周期在 computeVelocityCommands() 内调用 goal_checker->isGoalReached() 来判断。具体看下一步的伪代码。

---

getTolerances(pose_tolerance, vel_tolerance) → bool

对外公开的容差查询接口，用于返回该插件允许的位置误差和速度误差。

返回 bool 标记这些容差是否有效，无效则需走兜底逻辑。

• Pose 和 Twist 里字段很多，不是所有 GoalChecker 都检查所有字段
• 未被检查的字段 （如不看 z 轴、某些速度分量）会被填为 std::numeric_limits<double>::lowest()（double 极小负数，表示"不测量"）
• 组合容差（如 XY 平面距离）：X 和 Y 两个字段都写入同一个组合容差值

ai给出的解释： isGoalReached 是阅卷给出最终结论（到了 / 没到），getTolerances 是事先公布评分标准（差多少算到）。

---

第六步：局部控制器

controller.hpp

核心方法： setPlan(path) + computeVelocityCommands(pose, velocity, goal_checker) → TwistStamped

setPlan(path)

相当于 computeVelocityCommands 的初始化步骤：

1. 接收全局规划器输出的路径
2. 将路径存入内部变量
3. 重置到达标志位、路径索引等状态

触发时机：首次设置目标、切换目标、或判断当前路径无效需重规划时。

computeVelocityCommands(pose, velocity, goal_checker) → TwistStamped

每个控制周期调用一次，根据当前位姿和速度计算输出速度指令。
依赖 GoalChecker ，在内部调用 goal_checker->isGoalReached() 判断是否到达。

以纯追踪（Pure Pursuit）控制器为例的伪代码：

函数 computeVelocityCommands(pose, velocity, goal_checker):
  若未初始化 或 路径为空 或 位姿获取失败:
    返回 false（或抛出 PlannerException）

  获取机器人当前位姿
  更新 path_index（跳过已走过的路径点）
  选择前瞻点 target_pose

  若 goal_checker->isGoalReached(pose, goal_pose, velocity):
    输出零速度
    返回（标记任务完成）

  若接近终点:
    线速度 = 0，仅做角度对齐
    角速度按朝向误差 PI 控制
    返回 true

  否则:
    计算到前瞻点的位置误差与朝向误差
    用 PI 控制器计算线速度 v 和角速度 w，并限幅
    若朝向误差过大 → 原地转向（v = 0）

  填写 TwistStamped 并返回

setSpeedLimit(speed_limit, percentage)

限制机器人最大速度：

• percentage = true：以最大速度的百分比设置限制
• percentage = false：以绝对值（m/s）设置限制

---

第七步：恢复行为

behavior.hpp

只有生命周期接口（configure / activate / deactivate / cleanup），无业务方法------业务逻辑由子类自己定义（通常通过 Action Server 接收触发）。

注意与其他插件的区别： configure() 注入的不是 Costmap2DROS，而是 CostmapTopicCollisionChecker，因为恢复行为只需要做碰撞检测，不需要完整的代价地图规划接口。

当 ProgressChecker 判定机器人卡住后，上层 server 会触发此类插件（如原地旋转、后退清障等）。

---

第八步：路径点任务

waypoint_task_executor.hpp

核心方法： processAtWaypoint(curr_pose, curr_waypoint_index) → bool

参数	含义
curr_pose	机器人到达航点时的位置和朝向
curr_waypoint_index	当前是第几个航点（从 0 开始）

• 返回 true：任务执行成功，继续前往下一个航点
• 返回 false：任务执行失败，触发 waypoint_follower 的失败策略（停止或跳过，取决于配置）

用途： 在机器人到达某个航点时，执行自定义任务逻辑。例如：拍照、识别二维码、控制机械臂取放货物等。

---

整体流水线总览

目标输入
  ↓
GlobalPlanner::createPlan(start, goal)     ←  全局路径规划（A*、Dijkstra 等）
  ↓
Smoother::smooth(path, max_time)           ←  路径平滑（可选）
  ↓
Controller::setPlan(path)                  ←  将路径交给局部控制器
  ↓
Controller::computeVelocityCommands()      ←  实时输出速度指令（每控制周期）
  ├── ProgressChecker::check()             ←  机器人是否在前进？（防卡死）
  └── GoalChecker::isGoalReached()         ←  是否已到达目标？
  ↓
[卡住时]   Behavior::*                     ←  恢复行为（旋转、后退等）
[到航点时] WaypointTaskExecutor::processAtWaypoint()  ←  自定义任务

异常体系独立于流水线，贯穿全程：

• PlannerException：规划/控制过程中的通用异常
• RouteException 及其子类：路由规划（nav2_route）专用异常

---

下一步

nav2_core 读完后，可以按以下顺序深入具体实现：

1. nav2_navfn_planner → 看 GlobalPlanner 的最简实现（Dijkstra/A*）
2. nav2_controller → 看 Controller server 如何调用插件
3. nav2_dwb_controller 或 nav2_regulated_pure_pursuit_controller → 看 Controller 插件的完整实现
4. nav2_behaviors → 看 Behavior 插件（Spin、BackUp 等）