Skip to content
Anima Mundi Anima Mundi

添加 Nav2 任务服务器

Nav2 任务服务器（task server）包含处理不同类型请求的服务器端逻辑，通常由自主系统或行为树导航器（Behavior Tree Navigator）调用。本指南将介绍向 Nav2 添加新任务服务器所需的核心组件（如控制器（Controller）、行为（Behavior）、平滑器（Smoother）、规划器（Planner）服务器），包括如何设置新的生命周期组件节点（Lifecycle-Component Node）来完成启动和状态管理，以及如何（在需要时）传递语义明确的错误码。

本教程不涉及如何编写配套的行为树节点来与任务服务器交互，相关内容详见编写新的 BT 插件。通过该教程，你可以在 BT 导航器（BT Navigator）的行为树中调用自定义任务服务器。

如果你创建的新任务服务器对社区有普遍复用价值，欢迎联系维护者将其纳入 Nav2 项目。Nav2 的成长离不开像你这样的用户贡献。

生命周期节点（Lifecycle Nodes）

Section titled “生命周期节点（Lifecycle Nodes）”

生命周期节点（lifecycle node）是 Nav2 任务服务器的第一个关键组件。它是 ROS 2 中引入的机制，用于系统化管理机器人运行中各节点的启动（bringup）和关闭。借助生命周期节点，可以确保所有节点在开始执行前都已成功实例化；一旦存在无响应的节点，Nav2 会关闭全部节点。

生命周期节点内置状态机转换，使 ROS 2 服务器具有确定性行为。Nav2 中的生命周期节点转换由生命周期管理器（Lifecycle Manager）负责，它控制节点状态的转换，从而更好地掌控系统状态。

生命周期节点的主要状态包括 Unconfigured（未配置）、Inactive（未激活）、Active（激活）和 Finalized（终结）。节点实例化后从 Unconfigured 状态开始。生命周期管理器执行 Configurating（配置）转换，将节点从 Unconfigured 切换到 Inactive。该转换会设置所有配置参数，并完成必要的初始化工作，例如内存分配以及静态发布和订阅话题的建立。处于 Inactive 状态的节点可以重新配置参数，但不能执行任何处理操作。随后，生命周期管理器执行 Activating（激活）转换，将节点从 Inactive 切换到 Active——这是节点的主要工作状态，此时可以执行任何处理操作。如果节点崩溃，生命周期管理器会关闭整个系统以防止严重故障。在关闭过程中会执行必要的清理操作，节点依次经过 Deactivating（停用）、CleaningUp（清理）和 ShuttingDown（关闭）转换，最终进入 Finalized 状态。

参阅：有关生命周期管理的更多信息，请参阅关于托管节点（Managed Nodes）的文章。

你可能希望将自定义节点集成到 Nav2 框架中，或为系统添加新的生命周期节点。下面以一个虚构的生命周期节点 sensor_driver 为例，展示如何通过 Nav2 生命周期管理器对其进行控制，以确保在激活导航之前传感器数据流已就绪。具体做法是在启动文件中添加 sensor_driver 节点，并将其加入 lifecycle_manager 在导航前需要激活的节点列表中，如下例所示。

lifecycle_nodes = ['sensor_driver',
                   'controller_server',
                   'smoother_server',
                   'planner_server',
                   'behavior_server',
                   'bt_navigator',
                   'waypoint_follower']


...


Node(
    package='nav2_sensor_driver',
    executable='sensor_driver',
    name='sensor_driver',
    output='screen',
    parameters=[configured_params],
    remappings=remappings),


Node(
    package='nav2_lifecycle_manager',
    executable='lifecycle_manager',
    name='lifecycle_manager_navigation',
    output='screen',
    parameters=[{'autostart': autostart},
                {'node_names': lifecycle_nodes}]),

在上面的代码片段中，由生命周期管理器管理的节点通过 node_names 参数指定。该参数接收一个有序的节点列表，按生命周期转换的顺序依次启动。如代码片段所示，node_names 参数接收的就是包含目标节点列表的 lifecycle_nodes。生命周期管理器会逐个、按顺序对节点执行启动转换（Configuring 和 Activating），关闭转换时则按相反顺序处理。因此，sensor_driver 被排在其他导航服务器之前，以确保导航服务器激活之前传感器数据已就绪。

生命周期管理器还有另外两个参数：autostart 和 bond_timeout。如果希望在启动时自动将节点转换到 Active 状态，请将 autostart 设为 true；否则需要手动触发生命周期管理器来完成状态转换。bond_timeout 用于设置超时等待时间，当某个节点无响应超过该时间时，生命周期管理器会关闭所有节点。

参阅：有关生命周期管理器参数的更多信息，请参阅生命周期管理器配置指南。

组合（Composition）

Section titled “组合（Composition）”

组合（Composition）是 Nav2 任务服务器的第二个关键组件，它通过将多个节点放入单个进程中来减少内存和 CPU 开销。在 Nav2 中，可以使用组合将所有 Nav2 节点运行在单个进程中，而不必分别启动。这对于在嵌入式系统上部署、需要优化资源占用的场景尤为有用。

参阅：有关组合的更多信息，请参阅此处。

下面以一个名为 route_server 的示例，演示如何向系统添加新的 Nav2 服务器。

我们可以在启动文件中将不同的服务器组合到单个进程中。该进程由 ComposableNodeContainer 容器建立，再通过 ComposableNode 向容器中填充组合节点。之后，该容器就可以像其他 Nav2 节点一样被启动和使用。

  1. 在启动文件中添加一个新的 ComposableNode() 实例，指向你选择的组件容器。

    container = ComposableNodeContainer(
        name='my_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='nav2_route_server',
                plugin='nav2_route_server::RouteServer',
                name='nav2_route_server'),
        ],
        output='screen',
    )

    参阅：请参阅组合演示中的 composition_demo.launch.py 示例。

  2. 将包含该服务器的包添加到你的 package.xml 文件中。

    <exec_depend>nav2_route_server</exec_depend>

错误码（Error codes）

Section titled “错误码（Error codes）”

Nav2 任务服务器还可以在动作响应中返回 error_code 和 error_msg（非必需）。当系统存在语义明确、可操作的故障类型时，这是一种系统化的故障传达机制——故障信息可自动聚合到导航系统返回给应用程序的响应中。

0–9999 的错误码保留给 Nav2 内部服务器使用，每个服务器偏移 100；外部服务器则从 10000 开始，到 65535 结束。 下表列出了当前各服务器及其错误码结构。

服务器名称保留值范围
…NONE=0, UNKNOWN=12-99
Controller ServerNONE=0, UNKNOWN=100101-199
Planner Server（compute_path_to_pose）NONE=0, UNKNOWN=200201-299
Planner Server（compute_path_through_poses）NONE=0, UNKNOWN=300301-399
……
Smoother ServerNONE=0, UNKNOWN=500501-599
Waypoint Follower ServerNONE=0, UNKNOWN=600601-699
Behavior ServerNONE=0701-799
Coverage ServerNONE=0, UNKNOWN=800801-899
……
Last Nav2 ServerNONE=0, UNKNOWN=89008901-8999
……
Navigator - (nav_to_pose)NONE=0, UNKNOWN=90009001-9099
Navigator - (nav_thru_poses)NONE=0, UNKNOWN=91009101-9199
Navigator - Last NavigatorNONE=0, UNKNOWN=99009901-9999
……
First External ServerNONE=0, UNKNOWN=1000010001-10099
……

错误码和错误消息附加在动作消息的响应中。下面是 route server 的示例。注意，消息结果定义中错误码字段需命名为 error_code，错误消息字段需命名为 error_msg。

Terminal window
# Error codes
# Note: The expected priority order of the errors should match the message order
uint16 NONE=0 # 0 is reserved for NONE
uint16 UNKNOWN=10000 # first error code in the sequence is reserved for UNKNOWN


# User Error codes below
int16 INVALID_START=10001
int16 NO_VALID_ROUTE=10002


#goal definition
route_msgs/PoseStamped goal
route_msgs/PoseStamped start
string route_id
---
#result definition
nav_msgs/Route route
builtin_interfaces/Duration route_time
uint16 error_code
string error_msg
---

如消息中所述，错误码的优先级顺序应与消息定义顺序一致：0 保留给 NONE，序列中的第一个错误码保留给 UNKNOWN。 由于 route server 是外部服务器，其错误码从 10000 开始，最高到 10099。

要确保服务器的错误码及关联的错误消息在整个系统中被正确传达，需要在 nav2_params.yaml 文件中进行配置。

BT Navigator 参数 error_code_name_prefixes 定义了一个前缀列表，用于在行为树黑板（blackboard）中搜索可能已生成的错误码和错误消息键。如果黑板中包含多个错误码键，则序列中值最小的错误码及其关联的错误消息会被返回到导航器动作消息的结果中。错误码在软件栈中的层级越高，分配的值就越大——换言之，越底层的故障错误码值越小，优先级越高。

error_code_name_prefixes:
  - assisted_teleop
  - backup
  - compute_path
  - dock_robot
  - drive_on_heading
  - follow_path
  - nav_thru_poses
  - nav_to_pose
  - spin
  - route
  - undock_robot
  - wait

结论

Section titled “结论”

本指南介绍了生命周期节点、组合和错误码这三个 ROS 2 中的重要概念，并演示了如何在自定义节点/服务器上配合 Nav2 实现它们。这三项机制有助于系统高效运行，建议在 Nav2 中全面采用。