Nav2 的 bt_navigator 启动竞态——等 action server 就绪再激活

Nav2 启动后偶尔导航不工作。看日志，bt_navigator 报 “backup action server not available”，然后加载 BT XML 失败，节点进入 inactive 状态。重启又好了——典型的竞态条件。

这个问题困扰了我们两周。后来写了个 200 行的脚本彻底解决了。

问题根因

Nav2 的启动流程是这样的：

Lifecycle Manager 按配置顺序激活节点：controller_server → planner_server → behavior_server → bt_navigator → waypoint_follower
bt_navigator 激活时加载 BT XML 文件，BT XML 里引用了 BackUp、Spin、Wait 等 action
bt_navigator 尝试注册这些 action client，发现 behavior_server 的 action server 还没就绪
加载失败 → bt_navigator 变成僵尸节点

为什么 action server 没就绪？因为 Lifecycle Manager 激活 behavior_server 后，节点状态虽然变成了 active，但 action server 的注册和发布需要几百毫秒。Lifecycle Manager 不等这个——它立即去激活下一个节点。

这个时序在性能好的机器上几乎不出现，在树莓派上（CPU 慢、DDS 发现延迟大）出现率超过 30%。

解法：把 bt_navigator 从 Lifecycle Manager 里摘出来

关键思路：Lifecycle Manager 只管除了 bt_navigator 以外的节点。等所有 action server 确实就绪了，再手动 configure + activate bt_navigator。

第一步：Launch 配置排除 bt_navigator

# bringup.launch.py
lifecycle_nodes = [
    'controller_server',
    'planner_server',
    'behavior_server',
    # 'bt_navigator',  ← 注释掉，不归 lifecycle_manager 管
    'waypoint_follower',
    'smoother_server',
]

第二步：等 action server 就绪

ACTION_SERVERS = {
    'back_up': BackUp,
    'spin': Spin,
    'wait': Wait,
    'drive_on_heading': DriveOnHeading,
    'assisted_teleop': AssistedTeleop,
    'follow_path': FollowPath,
    'compute_path_to_pose': ComputePathToPose,
    'compute_path_through_poses': ComputePathThroughPoses,
    'smooth_path': SmoothPath,
}

class ActivateBtNavigator(Node):
    def __init__(self):
        self._action_clients = {}
        for name, action_type in ACTION_SERVERS.items():
            self._action_clients[name] = ActionClient(self, action_type, name)

        self._phase = 'waiting'
        self._timer = self.create_timer(2.0, self._tick)

    def _check_servers(self):
        ready, not_ready = [], []
        for name, client in self._action_clients.items():
            if client.server_is_ready():
                ready.append(name)
            else:
                not_ready.append(name)

        if not not_ready:
            self._phase = 'configuring'

每 2 秒检查一次，9 个 action server 全部 server_is_ready() 才进入下一步。超时 120 秒放弃。

第三步：手动 configure + activate

def _do_configure(self):
    req = ChangeState.Request()
    req.transition.id = Transition.TRANSITION_CONFIGURE
    future = self._change_state_cli.call_async(req)
    future.add_done_callback(self._on_configure_done)
    self._phase = 'configure_pending'

def _do_activate(self):
    req = ChangeState.Request()
    req.transition.id = Transition.TRANSITION_ACTIVATE
    future = self._change_state_cli.call_async(req)
    future.add_done_callback(self._on_activate_done)
    self._phase = 'activate_pending'

通过 bt_navigator/change_state 服务调用 ROS2 生命周期状态机，先 CONFIGURE 再 ACTIVATE——跟 Lifecycle Manager 内部做的事一模一样，只是我们手动控制了时序。

第四步：启动脚本加入 launch

# navigation_base.launch.py
Node(
    package='navigation',
    executable='wait_for_action_servers',
    name='activate_bt_navigator',
    arguments=['--ros-args', ...],
),

在 lifecycle_manager 之后启动。它自己会等，不会阻塞其他节点。

为什么不用 sleep

你可能会想：在 launch 文件里加个 TimerAction(delay=10.0) 不就行了？

不行。10 秒在大部分时候够了，但在 CPU 占用高的时候不够——behavior_server 可能 12 秒才就绪。sleep 太短问题还在，太长拖慢启动。而且 sleep 不检查实际状态——即使 action server 1 秒就准备好了，也得干等 10 秒。

主动轮询 server_is_ready() 是唯一可靠的方法：action server 一好就继续，不好就一直等，有超时兜底。

还要等 service

除了 action server，bt_navigator 还依赖一些 service，比如 reinitialize_global_localization（AMCL 重定位服务）。如果这个 service 没好，BT 里的 ComputePathToPose 节点可能在运行时找不到服务客户端。

self.declare_parameter('required_services', ['/reinitialize_global_localization'])

for service_name in self._required_services:
    self._service_clients[service_name] = self.create_client(Empty, service_name)

# 在 _check_servers 里一起检查
if (not not_ready) and (not not_ready_services):
    self._phase = 'configuring'

service 列表通过参数配置，不同部署可以加不同的依赖。

效果

改之前：树莓派上 30%+ 概率启动失败，需要手动重启 Docker。改之后：100% 成功启动，额外等待时间 2~8 秒（取决于 action server 就绪速度）。

这个 workaround 不是最优雅的——理想情况下 Nav2 的 Lifecycle Manager 应该自己处理这个竞态。但在他们修之前，200 行脚本是性价比最高的解法。