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

navigation2-humble从零带读笔记第一篇nav2_core免责声明本文内容为笔者从零学习 Nav2 的学习笔记为结合官方注释、个人理解及 AI 辅助解析整理而成。若存在解读偏差欢迎大家指正我会及时修正完善。nav2_core 的本质nav2_core 是纯抽象接口包——只有头文件没有任何实现代码。每个类的核心业务方法均为纯虚函数 0此外还有虚析构函数和Ptr类型别名(管理的共享指针)。它定义了 Nav2 所有插件必须遵守的规则纯虚基类。推荐阅读顺序按导航流水线顺序阅读每个接口对应一个功能环节。第一步基础异常exceptions.hpproute_exceptions.hppexceptions.hpp只有一个PlannerException继承自std::runtime_error出错时抛出异常对象用于 catch 捕获。整个是常规的 C 异常处理机制知道即可。try{// 路径规划相关代码thrownav2_core::PlannerException(路径规划失败);}catch(constnav2_core::PlannerExceptione){// 将异常描述信息输出到标准错误流std::cerre.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_limitsdouble::lowest()double 极小负数表示不测量组合容差如 XY 平面距离X 和 Y 两个字段都写入同一个组合容差值ai给出的解释isGoalReached是阅卷给出最终结论到了 / 没到getTolerances是事先公布评分标准差多少算到。第六步局部控制器controller.hpp核心方法setPlan(path)computeVelocityCommands(pose, velocity, goal_checker) → TwistStampedsetPlan(path)相当于computeVelocityCommands的初始化步骤接收全局规划器输出的路径将路径存入内部变量重置到达标志位、路径索引等状态触发时机首次设置目标、切换目标、或判断当前路径无效需重规划时。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 读完后可以按以下顺序深入具体实现nav2_navfn_planner→ 看 GlobalPlanner 的最简实现Dijkstra/A*nav2_controller→ 看 Controller server 如何调用插件nav2_dwb_controller或nav2_regulated_pure_pursuit_controller→ 看 Controller 插件的完整实现nav2_behaviors→ 看 Behavior 插件Spin、BackUp 等