Galactic to Humble

从 ROS 2 Galactic 迁移到 Humble 后，我们增加了许多稳定性改进，但在此我们不会特别介绍。

Smac Planner 的重大改进

Smac Planner 得到了显著改进，包括 2D 和 Hybrid-A* 实现，使路径更好、更快、质量更高。

碰撞检查器更快地拒绝碰撞，并减少查询代价地图坐标的次数
零拷贝碰撞检查对象
预先计算碰撞检查足迹方向，因此运行时无需触发
仅当机器人位于可能刻划的区域时才检查完整的 SE2 足迹
计算可能刻划的区域，或足迹的某些部分可能与边界发生碰撞的成本，以检查完整的足迹。否则，检查中心成本，因为承诺不会处于潜在碰撞状态
将 Hybrid-A* 规划器重命名为 SmacPlannerHybrid
离线预先计算 Reedshepp 和 Dubin 路径，因此在运行时它只是一个查找表
用一种新的、新颖的启发式方法（称为障碍启发式）取代波前启发式。这将使用差分 A* 搜索计算 Dijkstra 路径，同时考虑 8 个连通空间，以及位置成本的权重，以将启发式引导到过道的中心。它还对成本地图进行了下采样，这样就可以将扩展次数减少 75%，并且通过最多偏离单个单元格距离的部分分数，将非常小的误差引入启发式算法
改进分析扩展算法，以消除路径末端出现循环的可能性，尽可能消除
改进分析扩展以提供最大路径长度，以防止绕过障碍物
2D A* 行程成本和启发式改进，以加快规划时间并显着提高路径质量
用定制的梯度下降实现替换更平滑
将规划器的常用实用程序抽象到实用程序文件中
调整成本函数
使用动态规划预先计算障碍启发式，仅扩展最小数量的节点
缓存启发式设置，在目标保持不变时使用缓存的障碍启发式值启用 25hz 规划速率
利用 dubin 和 reeds-sheep 空间中的对称性将缓存大小减少50% 以增加可用于启发式查找的窗口大小。
在所有方向箱中预先计算基元
SmacPlanner2D 参数现在全部可重新配置
Hybrid-A* 和 State Lattice 规划器现在完全可接受
Hybrid-A* 和 State Lattice 已重新添加路径平滑的参数化。
平滑器现在启用运动学上可行的边界条件。
State Lattice 支持原语类型的原语转换
增加了回顾性惩罚以加快规划器的速度，使其优先考虑较晚的搜索分支，而不是较早的搜索分支，在绝大多数情况下，较早的搜索分支几乎没有机会改善路径

这些改进的 tl;dr 是： - 计划速度是以前的 2-3 倍，几乎所有情况下都远低于 200 毫秒，使其与 NavFn 和 Global Planner 一样快（但现在在运动学上可行）。典型的规划时间不到 100 毫秒，甚至不使用缓存或降采样功能。 - 通过改进的平滑器和一种新颖的启发式方法，路径的质量显著提高，这种启发式方法可以隐式地将机器人引导至过道中心。这使得路径更平滑，并且尽可能远离障碍物。 - 使用缓存或降采样器参数化，几乎可以在任何大小的空间中轻松实现 50 毫秒内的路径规划。 - 平滑器现在能够进行更多细化，并且可以创建运动学上可行的边界条件，即使在倒车时也是如此。

进行了其他改进，包括“analytic_expansion_max_length”参数，以便分析扩展的潜在长度受到限制。如果长度太远，则拒绝此扩展。这可以防止不安全地将路径捷径到远离目标本身的更高成本区域，让搜索在分析扩展将其带回家之前接近目标。这绝不能小于所用最小转弯半径的 4-5 倍，否则规划时间将开始激增。

此外，如果您之前已有配置，则遍历成本和启发式成本计算已更新**需要重新调整惩罚函数**。算法的默认值也已适当调整以适应变化，以实现与以前类似的开箱即用行为（用作参考）。

Simple (Python) Commander

This PR 2411 introduces a new package to Nav2, called the nav2_simple_commander. It is a set of functions in an object, BasicNavigator, which can be used to build Nav2-powered autonomy tasks in Python3 without concerning yourself with the Nav2, ROS 2, or Action server details. It contains a simple API taking common types (primarily PoseStamped) and handles all of the implementation details behind the hood. For example, this is a simple navigation task using this API:

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!')

The full API can be found in the README of the package. A number of well commented examples and demos can also be found in the package’s source code at the link prior.

Reduce Nodes and Executors

为了让nav2充分利用ROS 2，我们需要尽量减少nav2中的节点和执行器的数量，这样可以提高性能。

This functionality has been discussed in the ticket #816, and carried out in

Remove client_node_ in class WaypointFollower : PR2441

Remove rclcpp_node_ in class MapSaver : PR2454

Remove bond_client_node_ in class LifecycleManager : PR2456

Remove node_ in class LifecycleManagerClient : PR2469

Remove rclcpp_node_ in class ControllerServer : PR2459, PR2479

Remove rclcpp_node_ in class PlannerServer : PR2459, PR2480

Remove rclcpp_node_ in class AmclNode : PR2483

Remove rclcpp_node_ and clinet_node_ in class Costmap2DROS : PR2489

Remove rclcpp_node_ in class LifecycleNode : PR2993

some APIs are changed in these PRs:

PR2489 removes arguments client_node, rclcpp_node and adds argument callback_group in the initialize function of class nav2_costmap_2d::Layer. callback_group is used to replace rclcpp_node.
PR2993 removes argument use_rclcpp_node `` in the constructor of class ``nav2_util::LifecycleNode.

API Change for nav2_core

PR 2976 changes the API for nav2_core::Controller and nav2_core::Smoother by replacing the use of shared pointer references (const shared_ptr<> &) to shared pointers (shared_ptr<>). 使用共享指针引用意味着共享指针计数器永远不会增加。

扩展 BtServiceNode 来处理服务结果

This PR 2481 and PR 2992 address the ticket and this ticket and adds a virtual on_completion() function to the BtServiceNode class (can be found here). 与已经存在的虚拟“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-Node 的当前状态。由于该函数可以访问服务的结果，因此返回的节点状态可能取决于这些服务结果。

引入“on_completion()”函数不会影响“BtServiceNode”的正常行为，因为如果服务交互成功完成，默认实现仍然只返回“BT::NodeStatus::SUCCESS”。

包括新的旋转垫片控制器插件

This PR 2718 introduces the new nav2_rotation_shim_controller. This controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the primary controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking.

旋转垫片控制器适用于：

可以原地旋转的机器人，例如差速机器人和全向机器人。
当开始跟踪与机器人当前航向明显不同的新路径时，优先原地旋转而不是“螺旋式向外”。
使用非运动学上可行的规划器，例如 NavFn、Theta* 或 Smac 2D（可行的规划器，例如 Smac Hybrid-A* 和 State Lattice，将从机器人的实际起始航向开始搜索，不需要旋转）。

Spawning the robot in Gazebo

This PR 2473 deletes the pkg nav2_gazebo_spawner inside nav2_bringup directory. Instead of nav2_gazebo_spawner the Node spawn_entity.py of gazebo_ros is recommended to spawn the robot in gazebo. 请注意

应使用“libgazebo_ros_init.so”和“libgazebo_ros_factory.so”启动 gazebo 才能正常工作。
spawn_entity 节点无法在启动文件中将 /tf 和 /tf_static 重新映射到 tf 和 tf_static，仅用于多机器人情况。通过在每个在 SDF 文件中发布变换的插件中的 ros2 标签下添加重新映射参数“<remapping>/tf:=tf</remapping>”和“<remapping>/tf_static:=tf_static</remapping>”解决了此问题。区分不同机器人的 tf 至关重要。

恢复行为超时

Nav2 中的恢复、旋转和备份现在在其 BT 节点中具有“time_allowance”端口，并在其操作中请求字段以指定超时。这有助于确保机器人在卡住或无法在合理的时间段内备份整个距离的情况下可以退出备份或旋转原始行为。

3 个 2D 规划器的新参数“use_final_approach_orientation”

Pull request 2488 adds a new parameter use_final_approach_orientation to the 3 2D planners (Theta*, SmacPlanner2D and NavFn), false by default. If true, the last pose of the path generated by the planner will have its orientation set to the approach orientation, i.e. the orientation of the vector connecting the last two points of the path. It allows sending the robot to a position (x,y) instead of a pose (x,y,theta) by effectively ignoring the goal orientation. 例如，下面，对于同一个目标，其方向指向屏幕左侧，``use_final_approach_orientation=false``（左）和``use_final_approach_orientation=true``（右）

../_images/use_final_approach_orientation_false.gif

../_images/use_final_approach_orientation_true.gif

SmacPlanner2D 和 Theta*：修复被忽略的目标方向

This pull request 2488 fixes the issue of the goal pose orientation being ignored (the end path pose orientation was always set to 0).

SmacPlanner2D、NavFn 和 Theta*：修复小路径角点情况

This PR 2488 ensures the planners are not failing when the distance between the start and the goal is small (i.e. when they are on the same costmap cell), and in that case the output path is constructed with a single pose.

更改并修复动态参数变化检测的行为

This and this PR modify the method used to catch the changes of dynamic parameters. The motivation was to fix the issue that void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) was called for every parameter change of every node leading to unwanted parameter changes if 2 different nodes had the same parameter name.

动态参数

新增动态参数：

This PR 2592 makes most of the Costmap2DROS parameters dynamic
This PR 2607 makes most of the Regulated Pure Pursuit parameters dynamic
This PR 2665 makes most of the Theta * Planner parameters dynamic
This PR 2704 makes Waypoint Follower, Planner Server, and Controller Server’s params reconfigurable

BT 动作节点异常更改

当 BT 动作节点因网络或动作服务器故障而抛出异常时，它们现在会返回状态代码“FAILURE”，以使行为树中的特定动作无法响应。这与之前的情况相反，之前从网络抛出的异常被发送到根树，这将被视为树无法响应的任务级故障。

BT 导航器 Groot 多个导航器

This PR 2627 creates separate parameters for groot monitoring for the NavToPose and NavThroughPoses navigator types so you can individually track the state of each behavior tree through the ZMQ publisher. This resolves a long-standing problem after we added multiple navigator types to BT Navigator that you could only view the nav to poses BT execution live. BT.CPP and Groot only support one static ZMQ stream at a time, so there is a bit of a quirk where you must locally reset Groot after switching trees in order to view the live stream of the Nav Through Poses BT, if in use. This is a state of the BT.CPP and Groot libraries and not something we can resolve within Nav2.

由于这个怪癖以及几乎肯定会有大量关于该主题的票证涌入，有人考虑未来完全弃用使用 Groot 进行实时 BT 监控。但是，Groot 将始终支持可视化行为树 XML 文件和修改，只是不会在机器人导航期间实时可视化 BT 执行。

删除了 RPP 中的运动限制

参数 max_linear_accel 和 max_linear_decel 以及控制器中的运动限制被删除，这会导致不稳定。如果您想要类似的行为，请使用 ROS 生态系统中可用的速度平滑器。

添加了更平滑的任务服务器

添加了一个新的任务服务器，它可以加载更平滑的插件并执行它们以提高现有计划路径的质量。可以使用 SmoothPath 操作节点从行为树调用平滑操作。 PR 2569 实现，PR 2875 添加了第一个使用它的插件和一个简单的平滑器。其他平滑器正在开发中，将来会添加。

删除了 RPP 中的使用接近速度缩放参数

删除了参数 use_approach_linear_velocity_scaling，改为始终启用，以帮助平稳过渡到目标。此 PR 2701 实现。

将 AMCL 运动模型重构为插件

This PR 2642 为 AMCL 中当前使用的不同运动模型创建插件。此功能使用户能够使用任何自定义运动模型，方法是将其创建为插件，并在 nav2_params.yaml 文件中将 robot_model_type 参数更改为插件的名称。这有助于使用自定义运动模型，而无需修改 AMCL 源代码。

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

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

这是一个很棒的功能想法，但从未完全实现，特别是在我们在 BT Navigator 服务器中引入多种导航器类型之后。我们遇到的主要问题是 Zero-MQ 阻止用户在同一过程中生成多种记录器类型。由于 BT nav 有多个服务器，因此在它们之间切换查看从未顺利完成，导致人们提交工单或出现令人讨厌的日志或 ZMQ 在后台崩溃。为此，BT.CPP 客户端不允许我们有一个干净的关闭过程，所以我们只能希望 ZMQ 能够正确处理这种情况，但它很少能做到。此外，Groot 一次仅支持可视化一种类型的树，因此对于经常在导航器类型之间切换的应用程序，不可能使用单个 groot 客户端，这会带来很大的挫败感。

因此，我在这里建议从 Nav2 中删除对 BT 的实时监控。我们仍然可以使用 Groot 来修改、可视化和通常使用行为树，唯一被删除的是实时查看正在执行的行为树，因为 Nav2 当前正在执行它（它曾经点亮当前节点的框）。无论如何，这具有可疑的价值，因为树的滴答声如此之快，很难可视化并获得有意义的见解，因为系统移动得如此之快。

仅当路径无效时才重新规划

This PR 2591 创建了两个新的条件 BT 节点，以便仅在路径无效时才进行重新规划，而不是不断重新规划。这些新节点已集成到默认 BT 中。

修复 CostmapLayer clearArea 反转参数逻辑

This PR 2772 修复了 CostmapLayer clearArea 函数的反转参数逻辑。因此纠正了 clearAroundRobot 和 clearExceptRegion 服务及其相应 BT 操作的行为。

动态组合

This PR 2750 为用户提供了基于 ROS2 动态组合的可选启动。它可用于在单个进程中组合所有 Nav2 节点，而不是单独启动这些节点，这对于由于资源限制严重而需要进行优化的嵌入式系统用户非常有用。它默认使用，但可以使用启动参数 use_composition:=False 禁用。

一些实验显示了动态组合的性能改进，并且 CPU 和内存由 psutil 捕获：

CPU: Intel(R) i7-8700 (6Cores 12Threads), Memory: 32GB

cpu(%)

memory(%)

normal multiple processes

44

0.76

dynamic composition (use component_container_isolated)

38

0.23

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

BT 取消节点

This PR 2787 为用户提供了一个抽象节点，用于为 Nav2 堆栈中存在的不同服务器（如控制器服务器、恢复服务器等）开发取消行为。首先，此 PR 还提供了 CancelControl 行为来取消给予控制器服务器的目标。作为 CancelControl 的补充，此 PR 2856 <https://github.com/ros-planning/navigation2/pull/2856>`_ 为用户提供了取消恢复的选项，例如 ``backup、spin 和 wait。

BT PathLongerOnApproach 节点

This PR 中，添加了一个名为 PathLongerOnApproach 的新 Decorator BT 节点，以提供检查和潜在处理由于给定目标接近度中的障碍物而生成的更长路径的功能。为了演示此功能，新的 BT navigate_to_pose_w_replanning_goal_patience_and_recovery.xml 将作为示例和现成的 BT，适用于希望优化其流程周期时间的特定应用程序。下面可以看到开发的 BT 的演示，其中机器人在接近目标时暂停，以查看动态障碍物是否移开。否则，它会执行重新规划：

障碍物根本没有清除，obstacle_clearance_time 为 3 秒：

../_images/nav2_patience_near_goal_and_clear_obstacle.gif

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

../_images/nav2_patience_near_goal_and_go_around.gif

BT TruncatePathLocal 节点

This PR 2753 中，添加了一个名为 TruncatePathLocal 的新 Action BT 节点，以提取机器人附近的有限长度路径部分以供使用，例如用于碰撞检查或计算量大的平滑器

约束平滑器

This PR 2753 中，添加了一个名为 nav2_constrained_smoother::ConstrainedSmoother 的新平滑器，以优化各种路径标准，例如平滑度或与障碍物的距离，保持最小转弯半径

以恒定速率重新规划，如果路径无效

This PR 2804 引入了一种新的行为树，该行为树以一致的重新规划导航到姿势，如果路径无效。为了促进新的行为树，引入了一个新的条件节点 PathExpiringTimer 以触发以一致的速率重新规划。

欧几里得距离 2D

This PR 2865 将整个 nav2 中的欧几里得距离计算更改为投射到 XY 平面（即丢弃与 Z 中的组件相关的任何信息）。

如果使用 Nav2 生态系统之外的自定义插件，这可能会巧妙地改变某些 BT 节点、BT 导航器、控制器服务器、规划器服务器和 RPP 的行为方式。

恢复到行为

This 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 和 Lifecycle Manager 中的重生支持

This PR 2752 在 Nav2 中启用重生支持。在启动文件中，您可以将 use_respawn 设置为 true 以启用崩溃服务器的重生。这仅在非组合系统中可用，因为在组合系统中，所有节点都在一个进程下，任何地方的崩溃都会导致一切崩溃（包括生命周期管理器本身）。即使将容器设置为重生，它也只会重生空容器，而不是将所有组件加载到其中。

该 PR 还使生命周期管理器能够检查系统是否因崩溃而关闭。如果是，它允许管理器检查服务器是否在给定的超时期限内重新上线。如果是，它会自动将系统重新转换为活动状态以自动继续执行其任务。

新的 Nav2 速度平滑器

This PR 2964 引入了 nav2_velocity_smoother，用于通过速度、加速度和死区约束平滑从 Nav2 到机器人控制器的速度命令。有关更多详细信息，请参阅 Velocity Smoother。它不包含在``nav2_bringup`` 包含的默认启动电池中。

目标检查器 API 已更改

This PR 2965 在 nav2_core::GoalChecker 类的初始化函数中添加了一个额外参数。额外参数是一个 costmap_ros 指针。这用于检查目标是否发生碰撞，以便我们可以避免向目标移动，并且可以使用某些 BT 插件启动重新规划。

添加了辅助遥操作

This PR 2904 为辅助遥操作添加了新行为以及两个新的 BT 节点 AssistedTeleop 和 CancelAssistedTeleop。