编写新的行为插件

概述
要求
教程步骤

概述

本教程将介绍如何创建您自己的行为插件。行为插件位于行为服务器中。与规划器和控制器服务器不同，每个行为都将托管其自己独特的操作服务器。规划器和控制器具有相同的 API，因为它们完成相同的任务。但是，恢复可用于执行各种各样的任务，因此每个行为都可以拥有自己独特的操作消息定义和服务器。这为行为服务器提供了极大的灵活性，可以实现任何可以想象的行为操作，而不需要其他重用。

要求

ROS 2 (binary or build-from-source)
Nav2 (Including dependencies)
Gazebo
Turtlebot3

教程步骤

1-创建一个新的行为插件

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

我们的示例插件实现了“nav2_core::Behavior”插件类。但是，我们在“nav2_behaviors”中有一个不错的操作包装器，因此我们在此应用程序中使用“nav2_behaviors::TimedBehavior”基类。此包装器类派生自“nav2_core”类，因此可以用作插件，但可处理所需的绝大多数 ROS 2 操作服务器样板。

来自“nav2_core”的基类提供了 4 种纯虚拟方法来实现行为插件。行为服务器将使用该插件来托管插件，但每个插件都将提供自己独特的动作服务器接口。让我们进一步了解编写行为插件所需的方法**如果您没有使用“nav2_behaviors”包装器**。

Virtual method	Method description	Requires override?
configure()	Method is called at when server enters on_configure state. Ideally this method should perform declarations of ROS parameters and initialization of behavior’s member variables. This method takes 4 input parameters: shared pointer to parent node, behavior name, tf buffer pointer and shared pointer to a collision checker.	Yes
activate()	Method is called when behavior server enters on_activate state. Ideally this method should implement operations which are necessary before the behavior goes to an active state.	Yes
deactivate()	Method is called when behavior server enters on_deactivate state. Ideally this method should implement operations which are necessary before behavior goes to an inactive state.	Yes
cleanup()	Method is called when behavior server goes to on_cleanup state. Ideally this method should clean up resources which are created for the behavior.	Yes

对于提供 ROS 2 操作接口和样板的“nav2_behaviors”包装器，我们有 4 种虚拟方法需要实现。本教程使用此包装器，因此这些是我们将要解决的主要元素。

Virtual method	Method description	Requires override?
onRun()	Method is called immediately when a new behavior action request is received. Gives the action goal to process and should start behavior initialization / process.	Yes
onCycleUpdate()	Method is called at the behavior update rate and should complete any necessary updates. An example for spinning is computing the command velocity for the current cycle, publishing it and checking for completion.	Yes
onConfigure()	Method is called when behavior server enters on_configure state. Ideally this method should implement operations which are necessary before behavior goes to a configured state (get parameters, etc).	No
onCleanup()	Method is called when behavior server goes to on_cleanup state. Ideally this method should clean up resources which are created for the behavior.	No
onActionCompletion()	Method is called when the action has completed. Ideally, this method should populate the action result.	No

在本教程中，我们将使用方法“onRun()”、“onCycleUpdate()”和“onConfigure()”来创建 SMS 行为。

为简洁起见，我们将跳过“onConfigure()”，但只声明参数。

在恢复过程中，“onRun()”方法必须设置任何初始状态并启动行为。对于我们的求助行为，我们可以在此方法中轻松计算出我们的所有需求。

Status 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，我们想要处理它。 command 包含一个字段 message，其中包含我们想要传达给母舰的消息。这是我们想要通过短信发送给运营中心战友的“呼救”消息。

我们使用 Twilio 服务来完成这项任务。请``创建一个帐户 <https://www.twilio.com/>`_ 并获取创建服务所需的所有相关信息（例如 account_sid、auth_token 和电话号码）。您可以在配置文件中将这些值设置为参数，与 onConfigure() 参数声明相对应。

我们使用 _twilio 对象从配置文件中发送带有您帐户信息的消息。我们发送消息并记录以显示消息是否发送成功。我们根据要返回给操作客户端的该值返回“FAILED”或“SUCCEEDED”。

由于我们的行为运行时间短，因此“onCycleUpdate()”非常简单。如果行为运行时间较长，例如旋转、导航到安全区域或离开困境并等待帮助，则此函数将检查超时或计算控制值。对于我们的示例，我们只是返回成功，因为我们已经在“onRun()”中完成了任务。

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

其余方法未使用并且不需要覆盖它们。

2- 导出行为插件

现在我们已经创建了自定义行为，我们需要导出行为插件，以便行为服务器可以看到它。插件在运行时加载，如果它们不可见，那么我们的行为服务器将无法加载它。在 ROS 2 中，导出和加载插件由“pluginlib”处理。

来到我们的教程，类“nav2_sms_bahavior::SendSms”作为我们的基类“nav2_core::Behavior”动态加载。

为了导出行为，我们需要提供两行

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

请注意，它需要 pluginlib 来导出插件的类。Pluginlib 将提供宏“PLUGINLIB_EXPORT_CLASS”，它完成导出的所有工作。

将这些行放在文件末尾是一种很好的做法，但从技术上讲，您也可以写在顶部。

下一步是在包的根目录中创建插件的描述文件。例如，我们教程包中的“behavior_plugin.xml”文件。此文件包含以下信息

library path: Plugin’s library name and it’s location.

class name: Name of the class.

class type: Type of class.

base class: Name of the base class.

description: Description of the plugin.

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

下一步是使用 cmake 函数“pluginlib_export_plugin_description_file()”导出“CMakeLists.txt”插件。此函数将插件描述文件安装到“share”目录并设置索引以使其可发现。

pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml)

插件描述文件也应该添加到“package.xml”中

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

编译并注册完成，接下来我们来使用这个插件。

3-通过 params 文件传递插件名称

要启用该插件，我们需要修改“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_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

with

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_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

在上面的代码片段中，您可以观察到我们在“send_sms”ROS 2 动作服务器名称下添加了 SMS 行为。我们还告诉行为服务器“send_sms”属于“SendSms”类型，并为其提供您的 Twilio 帐户的参数。

4- 运行行为插件

运行启用 Nav2 的 Turtlebot3 模拟。详细说明请参阅：ref:getting_started。以下是快捷命令：

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

In a new terminal run:

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