添加一个新的Nav2任务服务器

nav2任务服务器包括完成不同类型请求的服务器端逻辑，通常由自主系统或通过行为树导航器调用。在本指南中，我们将讨论将新的任务服务器添加到Nav2所需的核心组件（例如控制器、行为、平滑器、规划器服务器）。特别是如何设置新的生命周期组件节点以进行启动和状态管理，以及语义上有意义的错误代码的通信（如果需要）。

尽管本教程不涵盖如何添加与该新任务服务器交互的补充行为树节点，但在:ref:`writing_new_nbt_plugin`中详细介绍了该内容，因此可以在BT Navigator中调用该任务服务器。

如果您创建了一个可能对社区有普遍重用价值的新任务服务器，请考虑联系维护人员将其添加到Nav2项目中！Nav2通过像您这样的用户的贡献而变得更好！

生命周期节点

Lifecycle节点是nav2任务服务器的首要关键组件。Lifecycle节点在ROS 2中引入，用于系统地管理参与机器人操作的不同节点的启动和关闭。使用Lifecycle节点可以确保在节点开始执行之前，所有节点都成功实例化，并且如果有任何无响应的节点，Nav2会关闭所有节点。

生命周期节点包含状态机转换，以实现ROS 2服务器中的确定性行为。Nav2中的生命周期节点转换由``Lifecycle Manager``处理。Lifecycle Manager转换生命周期节点的状态，并对系统的状态提供更大的控制。

Lifecycle节点的主要状态有``Unconfigured``、Inactive、Active``和``Finalized。Lifecycle节点在实例化后以``Unconfigured``状态启动。Lifecycle管理器通过实现``Configurating``转换将节点从``Unconfigured``转换为``Inactive``。Configurating``转换设置所有配置参数并准备任何必需的设置，如内存分配以及静态发布和订阅主题的设置。处于``Inactive``状态的节点可以重新配置其参数，但无法执行任何处理。从``Inactive``状态开始，Lifecycle管理器实现``Activating``转换状态，将节点从``Inactive``转换为主状态``Active。处于``Active``状态的节点可以执行任何处理操作。如果节点崩溃，Lifecycle管理器将关闭系统以防止任何关键故障。关闭时，执行必要的清理操作，并通过``Deactivating``、``CleaningUp``和``ShuttingDown``转换状态将节点转换为``Finalized``状态。

参见

有关生命周期管理的更多信息，请参阅`Managed Nodes的文章<https://design.ros2.org/articles/node_lifecycle.html>`_。

您可能希望将自己的节点集成到Nav2框架中，或向系统添加新的生命周期节点。作为示例，我们将添加一个名为``sensor_driver``的新的虚构生命周期节点，并通过Nav2 Lifecycle管理器对其进行控制，以确保在激活导航之前传感器数据可用。您可以通过在启动文件中添加一个``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``参数设置要由Lifecycle Manager处理的节点。node_names``参数接受一个有序列表，通过生命周期转换将节点启动。如示例中所示，``node_names``参数接受包含要添加到Lifecycle Manager的节点列表的``lifecycle_nodes。Lifecycle Manager按顺序逐个实现启动转换（Configuring``和``Activating）以及按相反顺序处理节点的关闭转换。因此，``sensor_driver``在其他导航服务器之前列出，以便导航服务器激活之前传感器数据可用。

Lifecycle管理器的另外两个参数是``autostart``和``bond_timeout``。如果要在启动时将转换节点设置为``Active``状态，请将``autostart``设置为``true``。否则，您将需要手动触发Lifecycle管理器来进行系统转换。``bond_timeout``设置等待时间，以决定何时转换所有节点的状态，如果一个节点没有响应。

注解

有关Lifecycle Manager参数的更多信息，请参阅`Lifecycle Manager配置指南<https://navigation.ros.org/configuration/packages/configuring-lifecycle.html>`_。

组合

Composition 是 nav2 任务服务器的第二个关键组件，通过将多个节点放在单个进程中来减少内存和 CPU 资源的使用。在 Nav2 中，可以使用 Composition 将所有 Nav2 节点组合到单个进程中，而不是分别启动它们。这在嵌入式系统上部署时非常有用，开发人员需要优化资源使用情况。

参见

有关组合的更多信息可以在`这里 <https://docs.ros.org/en/rolling/Tutorials/Intermediate/Composition.html>`_ 找到。

在下一部分中，我们将给出一个示例，说明如何向我们的系统中添加一个新的 Nav2 服务器，我们暂时称之为 route_server。

我们使用启动文件将不同的服务器组合成一个单一进程。该进程由``ComposableNodeContainer``容器建立，通过``ComposableNode``添加组合节点。然后可以像其他Nav2节点一样启动和使用此容器。

在您的 launch 文件中添加一个新的 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 <https://github.com/ros2/demos/blob/master/composition/launch/composition_demo.launch.py>`_。

将包含服务器的包添加到您的 package.xml 文件中。
<exec_depend>nav2_route_server</exec_depend>

错误代码

您的 nav2 任务服务器可能还希望在其操作响应中返回一个 'error_code'（尽管不是必需的）。如果对于您的系统而言有语义上有意义且可执行的失败类型，这是一种系统化的方式来传达这些失败，这些失败可能会自动聚合到导航系统对您的应用程序的响应中。

需要注意的是，0-9999的错误代码保留给内部nav2服务器，每个服务器的偏移量为100，而外部服务器的错误代码从10000开始，到65535结束。下表显示了当前的服务器以及预期的错误代码结构。

服务器名称	保留	RANGE
...	NONE=0，UNKNOWN=1	2-99
控制器服务器	NONE=0，UNKNOWN=100	101-199
计划器服务器 (计算路径到姿态)	NONE=0，UNKNOWN=200	201-299
`规划器服务器`_（计算路径通过姿态）	NONE=0, UNKNOWN=300	301-399
...	...
平滑器服务器	NONE=0，UNKNOWN=500	501-599
路径点跟随服务器	NONE=0, UNKNOWN=600	601-699
行为服务器	NONE=0	701 - 799
...	...
上一个 Nav2 服务器	NONE = 0，UNKNOWN = 9900	9901-9999
第一个外部服务器	NONE=0，UNKNOWN=10000	10001 - 10099
...	...

错误码附加在动作消息的响应中。下面是一个路由服务器的示例。请注意，根据约定，我们在消息定义中将错误码字段设置为 error_code。

# 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 INVILAD_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
---

如消息中所述，错误的优先级顺序应与消息顺序匹配，0 保留为NONE，序列中的第一个错误代码保留为UNKNOWN。由于路由服务器是外部服务器，错误代码从10000开始，一直增加到10099。

为了将服务器的错误代码传播到系统的其他部分，必须将其添加到nav2_params.yaml文件中。BT导航器中的`error_code_id_names`定义了服务器在黑板上要查找的错误代码。然后返回序列中最低的错误代码 - 而代码枚举会随着软件堆栈的增加而增加 - 为较低级别的故障提供更高的优先级。

error_code_id_names:
    - compute_path_error_code_id
    - follow_path_error_code_id
    - route_error_code_id

结论

在本指南的这个部分中，我们讨论了ROS 2中的生命周期节点、组合和错误代码，这些是新的重要概念。我们还展示了如何将生命周期节点、组合和错误代码应用于使用Nav2创建的新节点/服务器。这三个概念有助于有效地运行您的系统，因此鼓励在Nav2中广泛使用它们。