Galactic 迁移指南

从 ROS 2 Galactic 迁移到 Humble 带来了大量稳定性改进，此处不再逐一展开。

Smac 规划器的重大改进

Smac 规划器（2D 和 Hybrid-A* 两种实现）得到了显著改进，生成的路径更好、更快、质量更高。

碰撞检查器拒绝碰撞的速度更快，查询代价地图坐标的次数更少
零拷贝（zero-copy）碰撞检查对象
预计算碰撞检查的足迹（footprint）朝向，无需在运行时进行三角函数计算
仅在机器人处于可能内切区域（possibly inscribed zones）时才检查完整的 SE2 足迹
计算可能内切区域（即足迹某部分可能与障碍物碰撞的代价阈值），仅当超过该阈值时才检查完整足迹；否则只需检查中心代价，即可确保不会发生碰撞
将 Hybrid-A* 规划器更名为 SmacPlannerHybrid
离线预计算 Reeds-Shepp 和 Dubin 路径，运行时只需查表
用全新的障碍物启发式（obstacle heuristic）取代波前（wavefront）启发式。它通过 Differential A* 搜索计算 Dijkstra 路径，考虑 8 连通空间及各位置代价权重，将启发式引导至通道（aisleway）中心。同时对代价地图进行降采样，可将扩展次数减少约 75%，启发式引入的误差极小，最多偏差不到一个栅格距离
改进解析扩展（analytic expansion）算法，在可能的情况下消除路径末端的回环
改进解析扩展，限制最大路径长度，防止贴近障碍物绕行
改进 2D A* 的通行代价与启发式，以加快规划时间并显著提高路径质量
用定制的梯度下降实现替换了平滑器
将规划器的公共工具抽象到 utils 文件中
调优代价函数
使用动态规划预计算障碍物启发式，只扩展最少数量的节点
缓存启发式设置：当目标保持不变时，使用缓存的障碍物启发式值，可实现 25Hz 的规划频率
利用 Dubin 和 Reeds-Shepp 空间中的对称性，将缓存大小减少 50%，以增大可用于启发式查询的窗口
预计算所有朝向分箱（orientation bins）的基元（primitives）
SmacPlanner2D 的参数现在都可以重新配置
Hybrid-A* 和 State Lattice 规划器现在都是完全可采纳（admissible）的
Hybrid-A* 和 State Lattice 重新加入了路径平滑的参数化
平滑器现在支持运动学可行的边界条件
State Lattice 支持原地转向基元类型
添加回溯惩罚（retrospective penalty）以加快规划速度：优先扩展较晚的搜索分支而非较早的分支，因为在绝大多数情况下，较早的分支几乎不可能改善路径

这些改进的总结（tl;dr）：

规划速度提升 2-3 倍，几乎所有情况下都远低于 200ms，已与 NavFn 和 Global Planner 相当（且运动学可行）。即使不使用缓存或降采样，典型规划时间也低于 100ms。
得益于改进的平滑器和全新启发式（会隐式地将机器人引导至通道中心），路径质量显著提高——路径更平滑，且尽可能远离障碍物。
配合缓存或降采样参数，几乎在任何规模的搜索空间中都能轻松实现低于 50ms 的路径规划。
平滑器的细化处理更丰富，即使在倒车时也能创建运动学可行的边界条件。

此外还新增了 analytic_expansion_max_length 参数，用于限制解析扩展的最大长度：若扩展距离过远则拒绝该扩展，防止路径在远离目标的高代价区域走不安全的捷径，让搜索先逼近目标，再由解析扩展完成最后一段路径。该值不应小于所用最小转弯半径的 4-5 倍，否则规划时间会急剧上升。

此外，通行代价和启发式代价的计算方式已更新，如果你已有配置，需要重新调整惩罚函数（penalty function）。算法默认值也已相应调整，以保持与之前类似的即开即用行为，供参考。

Simple（Python）Commander

PR 2411 为 Nav2 引入了 nav2_simple_commander 包。它将一组函数封装在 BasicNavigator 对象中，可用 Python3 构建基于 Nav2 的自主导航任务，无需关心 Nav2、ROS 2 或动作服务器（Action server）的底层细节。这套简单的 API 接受常见类型（主要是 PoseStamped），在底层处理所有实现细节。例如，下面是一个简单的导航任务示例：

def main():
    rclpy.init()
    navigator = BasicNavigator()

    # Set our demo's initial pose
    initial_pose = PoseStamped()
    ... populate pose ...
    navigator.setInitialPose(initial_pose)

    # Wait for navigation to fully activate
    navigator.waitUntilNav2Active()

    # Go to our demos first goal pose
    goal_pose = PoseStamped()
    ... populate pose ...
    navigator.goToPose(goal_pose)

    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        ... do something with feedback ...

        # Basic navigation timeout
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
            navigator.cancelNav()

    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Goal succeeded!')
    elif result == TaskResult.CANCELED:
        print('Goal was canceled!')
    elif result == TaskResult.FAILED:
        print('Goal failed!')

完整的 API 可以在该包的 README 中找到。在该包源码中（见上方链接）还可以找到大量注释完善的示例和演示。

减少节点和执行器

为了让 Nav2 更好地利用 ROS 2，需要尽量减少节点和执行器（executor）的数量，从而提升性能。

该功能已在议题 #816 中讨论，并在以下 PR 中实施：

移除 class WaypointFollower 中的 client_node_：PR2441
移除 class MapSaver 中的 rclcpp_node_：PR2454
移除 class LifecycleManager 中的 bond_client_node_：PR2456
移除 class LifecycleManagerClient 中的 node_：PR2469
移除 class ControllerServer 中的 rclcpp_node_：PR2459、PR2479
移除 class PlannerServer 中的 rclcpp_node_：PR2459、PR2480
移除 class AmclNode 中的 rclcpp_node_：PR2483
移除 class Costmap2DROS 中的 rclcpp_node_ 和 clinet_node_：PR2489
移除 class LifecycleNode 中的 rclcpp_node_：PR2993

这些 PR 中一些 API 发生了变化：

PR2489 移除了 nav2_costmap_2d::Layer 类初始化函数中的参数 client_node、rclcpp_node，并新增参数 callback_group。callback_group 用于取代 rclcpp_node。
PR2993 移除了 nav2_util::LifecycleNode 构造函数中的参数 use_rclcpp_node。

nav2_core API 变更

PR 2976 更改了 nav2_core::Controller 和 nav2_core::Smoother 的 API，将共享指针引用 (const shared_ptr<> &) 改为共享指针值传递 (shared_ptr<>)。此前使用共享指针引用时引用计数不会递增，改为值传递后则会正常递增。

扩展 BtServiceNode 以处理服务结果

PR 2481 和 PR 2992 解决了该议题和此议题，在 BtServiceNode 类中新增了虚函数 on_completion()（可在此处查看）。与已有的虚函数 on_wait_for_result() 类似，该函数可在子类中重写，以自定义对相关事件的响应。on_completion() 在 BtServiceNode 的服务交互成功完成后被调用。

/**
* @brief Function to perform some user-defined operation upon successful
* completion of the service. Could put a value on the blackboard.
* @param response can be used to get the result of the service call in the BT Node.
* @return BT::NodeStatus Returns SUCCESS by default, user may override to return another value
*/
virtual BT::NodeStatus on_completion(std::shared_ptr<typename ServiceT::Response>/*response*/)
{
  return BT::NodeStatus::SUCCESS;
}

返回的 BT::NodeStatus 会设置 BT 节点的当前状态。由于该函数可访问服务结果，返回的节点状态可以依赖这些结果。引入 on_completion() 不会影响 BtServiceNode 的正常行为——默认实现仍然是在服务交互成功完成后返回 BT::NodeStatus::SUCCESS。

新增 Rotation Shim 控制器插件

PR 2718 引入了 nav2_rotation_shim_controller。该控制器会检查机器人当前朝向与新收到路径之间的粗略角度差：若在阈值范围内，则将请求传递给主控制器执行；若超出阈值，则先将机器人旋转到路径朝向。一旦进入容差范围，该旋转缓冲控制器（rotation shim controller）便将控制权移交给主控制器插件。移交时机器人仍在旋转，由主控制器插件接管，从而平滑过渡到路径跟踪。

Rotation Shim 控制器适用于：

可以原地旋转的机器人，如差速驱动机器人和全向机器人。
开始跟踪一条与机器人当前朝向差异很大的新路径时，倾向于原地旋转而非「螺旋式绕出」的场景。
使用非运动学可行规划器（如 NavFn、Theta或 Smac 2D）的场景——运动学可行规划器（如 Smac Hybrid-A 和 State Lattice）会从机器人实际起始朝向开始搜索，无需旋转。

在 Gazebo 中生成机器人

PR 2473 删除了 nav2_bringup 目录中的 nav2_gazebo_spawner 包。推荐使用 gazebo_ros 的 spawn_entity.py 节点（链接）在 Gazebo 中生成机器人。使用注意事项：

要正常工作，Gazebo 应同时加载 libgazebo_ros_init.so 和 libgazebo_ros_factory.so。
spawn_entity 节点目前无法在 launch 文件中将 /tf 和 /tf_static 重映射为 tf 和 tf_static（仅用于多机器人场景）。解决方法是在 SDF 文件中每个发布变换的插件 ros2 标签下添加重映射参数 <remapping>/tf:=tf</remapping> 和 <remapping>/tf_static:=tf_static</remapping>，这对于区分不同机器人的 tf 至关重要。

恢复行为超时

Nav2 中的恢复行为（Recoveries）——spin 和 backup——现在在其 BT 节点中新增了 time_allowance 端口，并在动作中新增了请求字段，用于指定超时时间。这样可确保机器人在卡住或无法在合理时间内完成全部后退距离时，能够中止 backup 或 spin 行为。

面向 3 个 2D 规划器的新参数 use_final_approach_orientation

PR 2488 为 3 个 2D 规划器（Theta*、SmacPlanner2D 和 NavFn）新增了参数 use_final_approach_orientation，默认值为 false。设为 true 时，规划器生成路径的最后一个位姿朝向将被设为接近朝向（approach orientation），即连接路径最后两个点的向量朝向。通过有效忽略目标朝向，可将机器人发送到位置 (x,y) 而非完整位姿 (x,y,theta)。例如，对于朝向指向屏幕左侧的同一目标，use_final_approach_orientation=false（左）和 use_final_approach_orientation=true（右）的效果如下：

use_final_approach_orientation_false

SmacPlanner2D 和 Theta*：修复目标朝向被忽略的问题

PR 2488 修复了该问题：目标位姿朝向被忽略（路径末端位姿的朝向总是被设为 0）。

SmacPlanner2D、NavFn 和 Theta*：修复小路径边缘情况

PR 2488 确保当起点和终点之间距离很小时（即它们位于同一个代价地图栅格上），规划器不会失败，此时输出路径由单个位姿构成。

动态参数变更检测行为的修改与修复

该 PR 和该 PR 改进了捕获动态参数变更的方法，修复了 void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) 对每个节点的每次参数变更都会被调用的问题——此前当两个不同节点存在相同参数名时，会产生不必要的参数变更通知。

动态参数

新增的动态参数：

PR 2592 使大部分 Costmap2DROS 参数变为动态参数
PR 2607 使大部分 Regulated Pure Pursuit 参数变为动态参数
PR 2665 使大部分 Theta* 规划器参数变为动态参数
PR 2704 使 Waypoint Follower、Planner Server 和 Controller Server 的参数可重新配置

BT 动作节点异常变更

当 BT 动作节点因网络或动作服务器故障抛出异常时，现在会返回状态码 FAILURE，使行为树中该特定动作失败并做出响应。此前恰恰相反：网络异常会被向上传播到行为树根节点，被视为整个行为树无法处理的任务级失败。

BT Navigator Groot 多导航器

PR 2627 为 NavToPose 和 NavThroughPoses 导航器类型创建了独立的 Groot 监控参数，可通过 ZMQ 发布器单独跟踪每个行为树的状态。这解决了一个长期存在的问题：在为 BT Navigator 添加多种导航器类型之后，只能实时查看 NavToPose BT 的执行。由于 BT.CPP 和 Groot 一次只支持一个静态 ZMQ 流，有一个小限制：切换树之后，若要使用 NavThroughPoses BT 的实时流，需在本地重置 Groot 才能查看。这是 BT.CPP 和 Groot 库本身的限制，无法在 Nav2 内部解决。

由于这个限制以及可能由此引发的大量议题，未来计划完全弃用 Groot 进行 BT 实时监控。Groot 仍将支持行为树 XML 文件的可视化和修改，只是不再支持实时可视化机器人导航过程中的 BT 执行。

移除 RPP 中的运动学限制

参数 max_linear_accel 和 max_linear_decel 以及控制器中导致不稳定的运动学限制已被移除。如需类似行为，请使用 ROS 生态系统中可用的速度平滑器（velocity smoother）。

新增平滑器任务服务器

新增了一个任务服务器，用于加载平滑器插件并执行，以提高现有规划路径的质量。可通过行为树中的 SmoothPath 动作节点调用平滑操作。PR 2569 实现了该功能，PR 2875 添加了首个使用该功能的插件（一个简单的平滑器）。其他平滑器正在开发中，将在未来陆续添加。

移除 RPP 中的接近速度缩放参数

参数 use_approach_linear_velocity_scaling 已被移除，该功能现在始终开启，以帮助平滑过渡到目标。PR 2701 实现了该变更。

将 AMCL 运动模型重构为插件

PR 2642 为 AMCL 中当前使用的不同运动模型创建了插件机制。用户可将任何自定义运动模型创建为插件，只需将 nav2_params.yaml 文件中的 robot_model_type 参数改为该插件名称即可使用，无需修改 AMCL 源代码。

放弃对 Nav2 实时 Groot 监控的支持

https://github.com/ros-navigation/navigation2/pull/2696

这是一个很好的功能构想，但从未真正成功过，尤其是在 BT Navigator 服务器中引入多种导航器类型之后。主要问题在于 Zero-MQ 不允许在同一进程中创建多个 logger 类型。BT Navigator 有多个服务器，在它们之间切换查看时始终无法干净地交接，导致用户提交议题、出现烦人的日志或后台 ZMQ 崩溃。BT.CPP 客户端不允许干净地关闭进程，只能寄希望于 ZMQ 能正确处理，但它很少能做到。此外，Groot 一次只支持可视化一种树，因此对于经常在导航器类型之间切换的应用，无法使用单一的 Groot 客户端，这造成了极大的困扰。

因此，建议移除 Nav2 的 BT 实时监控。仍可使用 Groot 来修改、可视化以及一般性地处理行为树，唯一被移除的是实时查看 Nav2 正在执行的行为树（即点亮当前节点方框的功能）。这本身价值有限，因为树的 tick 速度极快，系统运行速度极高，难以从中获得有意义的可视化洞察。

仅在路径无效时重新规划

PR 2591 创建了两个新的条件 BT 节点，仅在路径失效时才重新规划，而非持续重新规划。这些新节点已集成到默认行为树中。

修复 CostmapLayer clearArea 的 invert 参数逻辑

PR 2772 修复了 CostmapLayer 的 clearArea 函数中 invert 参数的逻辑，从而纠正了 clearAroundRobot 和 clearExceptRegion 服务及其对应 BT 动作的行为。

动态组合

PR 2750 提供了基于 ROS 2 动态组合（dynamic composition）的可选 bringup 方案。它可以将所有 Nav2 节点组合在单个进程中，而非分别启动各节点，对于资源受限的嵌入式系统用户非常有用。该功能默认启用，可通过 launch 参数 use_composition:=False 禁用。

下面是一些展示动态组合性能提升的实验，CPU 和内存由 psutil 采集：

CPU: Intel(R) i7-8700（6 核 12 线程），内存：32GB	cpu(%)	memory(%)
普通多进程方式	44	0.76
动态组合（`component_container_isolated`）	38	0.23

与普通多进程方式相比，动态组合内存消耗更低（节省约 70%），CPU 也更低（节省约 13%）。

BT 取消节点

PR 2787 提供了一个抽象节点，用于为 Nav2 技术栈中不同服务器（如 controller_server、recovery_server 等）开发取消行为。作为起步，该 PR 还提供了 CancelControl 行为，用于取消已发送给 controller_server 的目标。作为 CancelControl 的补充，PR 2856 提供了取消恢复行为（如 backup、spin 和 wait）的选项。

BT PathLongerOnApproach 节点

该 PR 新增了名为 PathLongerOnApproach 的装饰器（Decorator）BT 节点，用于检测并处理因给定目标附近出现障碍物而产生的更长路径。为演示该功能，还新增了 BT navigate_to_pose_w_replanning_goal_patience_and_recovery.xml，既是示例也是即用型 BT，适用于希望优化流程周期时间的特定应用。该 BT 的演示效果如下：当机器人靠近目标时会暂停，观察动态障碍物是否会移开；若未移开，则执行重新规划。

障碍物完全未清除，obstacle_clearance_time 设为 3 秒：

nav2_patience_near_goal_and_clear_obstacle

障碍物被清除后，可以看到机器人沿（原本理想情况下相同的）路径通过：

nav2_patience_near_goal_and_go_around

BT TruncatePathLocal 节点

PR 2753 新增了名为 TruncatePathLocal 的动作 BT 节点，用于提取机器人附近的有界路径段，例如用于碰撞检查或计算量较大的平滑器。

约束平滑器（Constrained Smoother）

PR 2753 新增了 nav2_constrained_smoother::ConstrainedSmoother 平滑器，用于优化各种路径指标（如平滑度或距障碍物的距离），同时保持最小转弯半径。

以恒定频率重新规划且仅在路径无效时重新规划

此 PR 2804 引入了一个新的行为树，它能够在路径失效时重新规划的同时保持一致的重新规划频率，用于导航到位姿。为支持该行为树，还引入了一个新的条件节点 PathExpiringTimer，以恒定频率触发重新规划。

2D 欧几里得距离

此 PR 2865 改变了 Nav2 中欧几里得距离的计算方式，将其投影到 XY 平面（即丢弃 Z 分量的相关信息）。如果你使用了 Nav2 生态之外的自定义插件，这可能会使某些 BT 节点、BT 导航器、控制器服务器、规划器服务器和 RPP 的行为发生细微变化。

Recovery 更名为 Behavior

此 PR 2867 将 nav2_recoveries 更名为 nav2_behaviors。

在 navigation_launch.py 中：recoveries_server -> behavior_server，nav2_recoveries -> nav2_behaviors。在 nav2_params.yaml 中：recovery_plugins -> behavior_plugins，nav2_recoveries -> nav2_behaviors。

Launch 与生命周期管理器中的 Respawn 支持

PR 2752 在 Nav2 中启用了 respawn（进程重启）支持。在 launch 文件中，可以将 use_respawn 设置为 true，以启用崩溃服务器的自动重启。该功能仅适用于非组合（non-composed）系统——在组合系统中，所有节点都在单个进程内运行，任何位置崩溃都会导致整个进程（包括生命周期管理器本身）停止。即使将容器设置为 respawn，它也只能重启一个空容器，而不会重新加载其中的所有组件。

该 PR 还使生命周期管理器能够检测系统是否因崩溃而停机。如果是，管理器会在给定的超时时间内检查服务器是否重新上线。若重新上线，它会自动将系统恢复到 active 状态，继续执行任务。

新的 Nav2 速度平滑器

PR 2964 引入了 nav2_velocity_smoother，通过速度、加速度和死区（deadband）约束来平滑从 Nav2 输出到机器人控制器的速度命令。更多细节请参阅 configuring_velocity_smoother。该组件不包含在 nav2_bringup 默认 bringup 的「开箱即用」功能中。

Goal Checker API 变更

PR 2965 在 nav2_core::GoalChecker 类的初始化函数中增加了一个额外参数。该额外参数是一个 costmap_ros 指针。它用于检查目标是否处于碰撞状态，从而避免向发生碰撞的目标移动，并可通过某些 BT 插件发起重新规划。

新增辅助遥操作（Assisted Teleop）

PR 2904 新增了辅助遥操作（assisted teleop）行为，以及两个新的 BT 节点：AssistedTeleop 和 CancelAssistedTeleop。