编写新的行为插件

• 概述
• 需求
• 教程步骤

概述

本教程演示如何创建自定义行为插件（Behavior Plugin）。行为插件运行在行为服务器（behavior server）中。与规划器和控制器服务器不同，每个行为都拥有独立的动作服务器（action server）。规划器和控制器共享相同的 API，因为它们完成的是同一类任务；而恢复（recovery）行为可用于执行各种不同的任务，因此每个行为都可以拥有自己独有的动作消息定义和服务器。这赋予了行为服务器极大的灵活性，可以用来实现任意所需的恢复行为，而不必考虑接口的复用性。

需求

• ROS 2（二进制安装或源码编译）
• Nav2（包括依赖项）
• Gazebo
• TurtleBot3

教程步骤

1- 创建新的行为插件

我们将创建一个简单的发送短信（SMS）行为，使用 Twilio 通过 SMS 向远程操作中心发送消息。本教程中的代码可以在 navigation_tutorials 仓库中找到，即 nav2_sms_behavior 包。该包可作为编写行为插件的参考。

示例插件实现了 nav2_core::Behavior 插件类。不过，nav2_behaviors 为动作提供了一个便捷的包装器（wrapper），因此本教程使用 nav2_behaviors::TimedBehavior 基类。该包装类派生自 nav2_core 类，可以作为插件使用，同时处理了 ROS 2 动作服务器所需的大部分样板代码。

nav2_core 中的基类提供了 4 个纯虚方法来实现行为插件。行为服务器会加载各个插件，而每个插件提供自己独有的动作服务器接口。如果你没有使用 nav2_behaviors 包装器，下面介绍编写行为插件所需的方法。

虚方法	方法描述	需要重写？
configure()	当服务器进入 on_configure 状态时调用。通常在此方法中声明 ROS 参数并初始化行为成员变量。该方法接收 4 个参数：父节点（parent node）的共享指针、行为名称、tf 缓冲区指针以及碰撞检查器（collision checker）的共享指针。	是
activate()	当行为服务器进入 on_activate 状态时调用。通常在此方法中完成行为进入激活状态前所需的操作。	是
deactivate()	当行为服务器进入 on_deactivate 状态时调用。通常在此方法中完成行为进入非激活状态前所需的操作。	是
cleanup()	当行为服务器进入 on_cleanup 状态时调用。通常在此方法中清理行为所创建的资源。	是

nav2_behaviors 包装器提供了 ROS 2 动作接口和样板代码，使用它时需要实现以下虚方法。本教程使用该包装器，因此重点讨论这些方法。

虚方法	方法描述	需要重写？
onRun()	收到新的行为动作请求时立即调用。该方法接收动作目标（action goal），应启动行为的初始化和后续处理。	是
onCycleUpdate()	以行为更新频率调用，完成所有必要的周期更新。例如旋转（spinning）行为中，需要计算当前周期的指令速度、发布指令并检查是否完成。	是
onConfigure()	当行为服务器进入 on_configure 状态时调用。通常在此方法中完成行为进入已配置状态前所需的操作（如获取参数等）。	否
onCleanup()	当行为服务器进入 on_cleanup 状态时调用。通常在此方法中清理行为所创建的资源。	否
onActionCompletion()	当动作完成时调用。通常在此方法中填充动作结果（action result）。	否

在本教程中，将使用 onRun()、onCycleUpdate() 和 onConfigure() 方法来创建 SMS 行为。为简洁起见，onConfigure() 部分省略——它只声明参数。

在恢复（recovery）行为中，onRun() 方法负责设置初始状态并启动行为。对于本例的「呼叫求助」行为，可以在该方法中直接完成所有计算。

ResultStatus SendSms::onRun(const std::shared_ptr<const Action::Goal> command)
{
  std::string response;
  bool message_success = _twilio->send_message(
    _to_number,
    _from_number,
    command->message,
    response,
    "",
    false);

  if (!message_success) {
    RCLCPP_INFO(node_->get_logger(), "SMS send failed.");
    return ResultStatus{Status::FAILED};
  }

  RCLCPP_INFO(node_->get_logger(), "SMS sent successfully!");
  return ResultStatus{Status::SUCCEEDED};
}

command 是接收到的动作目标，其中包含 message 字段，即我们要传达给「母舰」的消息——这就是希望通过 SMS 发送给操作中心的「呼叫求助」信息。

该任务通过 Twilio 服务完成。请先创建账户并获取所需信息（如 account_sid、auth_token 和一个电话号码），然后在配置文件中将这些值设置为与 onConfigure() 参数声明对应的参数。

代码中使用 _twilio 对象，利用配置文件中的账户信息发送消息，并将发送结果记录到日志。根据发送是否成功返回 FAILED 或 SUCCEEDED，将结果传递给动作客户端。

由于该行为运行时间很短，onCycleUpdate() 非常简单。如果行为运行时间较长（如旋转、导航到安全区域、离开危险位置并等待救援），则需要在该函数中检查超时或计算控制值。在本例中，onRun() 已经完成了所有工作，因此直接返回成功即可。

ResultStatus SendSms::onCycleUpdate()
{
  return ResultStatus{Status::SUCCEEDED};
}

其余方法未使用，也不是必须重写的。

2- 导出行为插件

创建自定义行为后，需要导出行为插件，使行为服务器能够发现它。插件在运行时加载，如果未正确导出，行为服务器将无法加载。在 ROS 2 中，插件的导出和加载由 pluginlib 处理。

在本教程中，类 nav2_sms_behavior::SendSms 将作为基类 nav2_core::Behavior 的子类被动态加载。

1. 导出行为需要以下两行代码：

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_sms_behavior::SendSms, nav2_core::Behavior)

这里需要 pluginlib 来导出插件类。pluginlib 提供了宏 PLUGINLIB_EXPORT_CLASS，由它完成所有导出工作。

建议将这些代码放在文件末尾，但放在文件顶部也可以。

1. 下一步在包的根目录中创建插件描述文件。例如本教程包中的 behavior_plugin.xml 文件，该文件包含以下信息：

• library path：插件的库名称及其位置。
• class name：类的名称（可选）。如果未设置，将默认为 class type。
• class type：类的类型。
• base class：基类的名称。
• description：插件的描述。

<library path="nav2_sms_behavior_plugin">
  <class type="nav2_sms_behavior::SendSms" base_class_type="nav2_core::Behavior">
    <description>This is an example plugin which produces an SMS text message recovery.</description>
  </class>
</library>

1. 下一步是在 CMakeLists.txt 中使用 CMake 函数 pluginlib_export_plugin_description_file() 导出插件。该函数将插件描述文件安装到 share 目录，并设置 ament 索引使其可被发现。

pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml)

1. 插件描述文件也应添加到 package.xml 中。

<export>
  <build_type>ament_cmake</build_type>
  <nav2_core plugin="${prefix}/behavior_plugin.xml" />
</export>

1. 编译后插件即被注册。可通过以下命令验证是否注册成功：

$ ros2 plugin list

你应该会看到类似下面的输出：

nav2_sms_behavior:
      Plugin(name='nav2_sms_behavior::SendSms', type='nav2_sms_behavior::SendSms', base='nav2_core::Behavior')

接下来，我们将使用这个插件。

3- 通过参数文件传入插件名称

要启用该插件，需按如下方式修改 nav2_params.yaml 文件，将原有参数替换为：

behavior_server:  # Humble and later
recoveries_server:  # Galactic and earlier
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "wait"]  # Humble and later
    recovery_plugins: ["spin", "backup", "wait"]  # Galactic and earlier
    spin:
      plugin: "nav2_behaviors::Spin" # In Iron and older versions, "/" was used instead of "::"
    backup:
      plugin: "nav2_behaviors::BackUp" # In Iron and older versions, "/" was used instead of "::"
    wait:
      plugin: "nav2_behaviors::Wait" # In Iron and older versions, "/" was used instead of "::"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

替换为：

behavior_server:  # Humble and newer
recoveries_server:  # Galactic and earlier
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_costmap_topic: global_costmap/costmap_raw
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "wait","send_sms"]  # Humble and newer
    recovery_plugins: ["spin", "backup", "wait","send_sms"]  # Galactic and earlier
    spin:
      plugin: "nav2_behaviors::Spin" # In Iron and older versions, "/" was used instead of "::"
    backup:
      plugin: "nav2_behaviors::BackUp" # In Iron and older versions, "/" was used instead of "::"
    wait:
      plugin: "nav2_behaviors::Wait" # In Iron and older versions, "/" was used instead of "::"
    send_sms:
      plugin: "nav2_sms_behavior::SendSms" # In Iron and older versions, "/" was used instead of "::"
    account_sid: ... # your sid
    auth_token: ... # your token
    from_number: ... # your number
    to_number: ... # the operations center number
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

在上面的代码中，SMS 行为被添加到 send_sms ROS 2 动作服务器名称下。同时告知行为服务器 send_sms 的类型为 SendSms，并提供了 Twilio 账户参数。

4- 运行行为插件

运行启用了 Nav2 的 TurtleBot3 仿真。详细说明请参阅快速入门，快捷命令如下：

$ ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/path/to/your_params_file.yaml

在新的终端中运行：

$ ros2 action send_goal "send_sms" nav2_sms_behavior/action/SendSms "{message : Hello!! Navigation2 World }"