一.Navigation2导航bt错误导致的现象

[bt_navigator-13] [INFO] [1745473498.377081813] [bt_navigator]: Begin navigating from current location to (0.20, 0.00)
[bt_navigator-13] [ERROR] [1745473498.400021834] [bt_navigator]: Action server failed while executing action callback: "send_goal failed"
[bt_navigator-13] [WARN] [1745473498.410767435] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.

在出现这个提示后，如果重新指定导航位置，出现如下提示，但无法进行导航也没有local plan的路径。小车直接卡死在原地，但雷达和代价地图等完全正常。

[bt_navigator-13] [INFO] [1745478913.039561443] [bt_navigator]: Begin navigating from current location to (0.26, -0.02)
[bt_navigator-13] [INFO] [1745478915.010285001] [bt_navigator]: Received goal preemption request
[bt_navigator-13] [INFO] [1745478915.010516570] [bt_navigator]: Begin navigating from current location to (0.20, 0.00)
[bt_navigator-13] [INFO] [1745478917.240272352] [bt_navigator]: Received goal preemption request
[bt_navigator-13] [INFO] [1745478917.240513035] [bt_navigator]: Begin navigating from current location to (1.24, -0.19)
[bt_navigator-13] [INFO] [1745478918.990284556] [bt_navigator]: Received goal preemption request
[bt_navigator-13] [INFO] [1745478918.990516072] [bt_navigator]: Begin navigating from current location to (0.20, 0.00)
[bt_navigator-13] [INFO] [1745478922.010273596] [bt_navigator]: Received goal preemption request
[bt_navigator-13] [INFO] [1745478922.010561208] [bt_navigator]: Begin navigating from current location to (1.35, 0.87)
[bt_navigator-13] [INFO] [1745478924.000307291] [bt_navigator]: Received goal preemption request
[bt_navigator-13] [INFO] [1745478924.000549277] [bt_navigator]: Begin navigating from current location to (0.20, 0.00) 

小车停着不动，也不能使用Navigation2 Goal工具重新指定位置：

二.原因

在 ROS 2 Foxy Patch Release 4 与 master 分支同步后，bt_navigator 在执行行为树节点的 send_goal 调用时，会由于默认的 10 ms server_timeout 超时，频繁报出。

核心原因在于行为树每个 Action 节点在初始化时都会使用 10 ms 作为 server_timeout，这一数值与默认的 bt_loop_duration（10 ms）完全匹配，导致只要 send_goal 耗时稍微超过 10 ms 就会触发超时，同时，ROS 2 Actions/Services 在 rclcpp 层面也出现了性能回归，平均 send_goal 耗时在 10 ms 以上（尤其在 aarch64 架构或低性能硬件（树莓派、Jetson等）上更为明显），进一步加剧了这一问题，虽然可以通过增大超时时间或降低行为树循环频率（增大 bt_loop_duration）来暂时规避，但这并不能从根本上解决性能瓶颈，只是权宜之计，该 Issue 最终于 2024 年 9 月被标记为 “Won’t Fix” 并关闭，官方建议如果在 Humble 及以后版本仍有问题，可另行提单；Foxy 已退出支持周期，不再规划修复。

由于ros2 foxy版本太旧，无人更新，这个问题从几年前一直被搁置到现在并且再也不会修复，为了能用导航我们做以下努力：

三.解决方案

1.方法一：在Nav2配置文件修改默认延时（推荐）

步骤1.添加修改Nav2 默认参数示例（nav2_params.yaml）：

bt_navigator:
  ros__parameters:
    bt_loop_duration： 50  # ms （默认 10 ms）调整行为树循环周期（单位 ms），建议将其增大到 20 ms 或 50 ms，以降低系统在高频率 Tick 下的性能压力
    default_server_timeout: 200   # ms （ 默认 20 ms）指定 BT Action 节点等待 Action Server 响应的超时时间。请将其设置为大于您的 bt_loop_duration（如 50 ms 或更高），以防止 send_goal 调用因超时而失败
    wait_for_service_timeout: 1000  # ms
    action_server_result_timeout: 900.0  # s  控制 Action Server 丢弃未在该时限内返回结果的 Goal，一般无需改动；如有长时任务，可适当增大

步骤2.修改BT xml文件

由于上面的延时默认参数会始终被覆盖，需要在 BT XML 中为关键节点显式添加
在 Behavior Tree XML 中为各个动作节点单独添加延时，防止超过时间 server_timeout：

<NavigateToPose
  server_timeout="200"
  … />
<ComputePathToPose
  server_timeout="200"
  … />
<FollowPath
  server_timeout="200"
  … />

实际操作中，打开默认的名称为navigate_w_replanning_and_recovery.xml的文件，在这两处地方添加：server_timeout=“200”

添加前：

<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>

添加后：

<ComputePathToPose server_timeout="200" goal="{goal}" path="{path}" planner_id="GridBased"/>

添加前：

<FollowPath path="{path}" controller_id="FollowPath"/>

添加后：

<FollowPath server_timeout="200" path="{path}" controller_id="FollowPath"/>

整个文件修改后：

<!--
  This Behavior Tree replans the global path periodically at 1 Hz and it also has
  recovery actions.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose server_timeout="200" goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath server_timeout="200" path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <SequenceStar name="RecoveryActions">
          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          <!-- <Spin spin_dist="1.57"/> -->
          <Wait wait_duration="5"/>
        </SequenceStar>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

最后，在Nav2配置文件中指定bt_xml文件路径

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom_combined
    
    bt_loop_duration: 50
    default_server_timeout: 200 
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0

    default_bt_xml_filename: "/home/nvidia/catkin_ws/src/car_nav2/config/navigate_w_replanning_and_recovery.xml"

在（左上右上右下左下）四个角点的循环测试，十轮共40次定点导航没有出现问题。

2.方法二：修改并重新编译Nav2源码（麻烦）

如果无法升级 ROS 2 或希望在 Foxy 上修复：

在 Nav2 源码 nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp 中，修改 server_timeout_ 的读取逻辑，先尝试从 BT 输入端口获取，若不存在再使用默认值：

if (!getInput<std::chrono::milliseconds>("server_timeout", server_timeout_)) {
  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
}

然后重新编译Nav2的源码包

注：该补丁已在 Galatic和Humble的Navigation2 主干分支合入，升级为Galactic或Humble可以直接解决此问题，可参见 Issue #4618

3.方法三：升级ROS2版本（从根源解决问题）

如果可以，直接升级到 ROS2 发行版 Galactic（Ubuntu20.04）或 Humble（Ubuntu 22.04 24.04），这两个版本已在 rclcpp 层面合入性能优化补丁，并在 Nav2 中将默认超时时间调整为更宽松的值，能有效避免此类超时问题。

四.希望能帮到大家

谢谢大家点赞