【Nav2中文网】六、插件教程（五）编写新的恢复插件

• 本教程来自：Nav2中文网
• Nav2交流社区：https://fishros.org.cn/forum
• ROS2/Nav2千人交流群：(QQ)139707339
• 更多精彩教程请关注微信公众号：鱼香ROS
• 欢迎添加机器人小伊微信，解锁机器人学习特殊服务

编写新的恢复插件 [待校准@2180]

• 概述
• 必要条件
• 教程步骤

概述

本教程演示如何创建自己的恢复插件。恢复插件位于恢复服务器中。与规划器和控制器服务器不同，每个恢复都将托管自己独特的操作服务器（action server）。规划器和控制器在完成相同任务时具有相同的API。但是，恢复可用于执行各种各样的任务，因此每个恢复都可以有自己独特的操作消息定义和服务器。这允许恢复服务器的巨大灵活性，实现任何可想象的不需要其他重用的恢复操作。 [校准@混沌无形]

必要条件

• ROS 2 (二进制或从源代码构建)
• Nav2 (包括依赖项) [待校准@1996]
• Gazebo [待校准@1997]
• Turtlebot3 [待校准@1998]

教程步骤

1-创建一个新的恢复插件 [待校准@2182]

我们将创建一个简单的呼救恢复行为。它将使用Twilio通过短信向远程操作中心发送消息。本教程中的代码在 navigation_tutorials 库 nav2_sms_recovery 。这个软件包可以被认为是编写恢复插件的参考。 [校准@混沌无形]

我们例子插件实现了 nav2_core::Recovery``的插件类。然而，我们在``nav2_recoveries``中有一个很好的动作包装器，所以我们在这个应用中使用``nav2_recoveries::Recovery``基类来代替。此包装类来自 ``nav2_core 类，可以作为一个插件，但处理绝大多数的ROS 2行动服务器所需的锅炉板。 [校准@混沌无形]

基类 nav2_core 提供4种纯虚拟方法执行恢复插件。插件将使用恢复服务器主机插件，但每个插件提供自己独特动作服务器接口。让我们了解更多关于编写恢复插件所需的方法 * * 如果你没有使用 nav2_recoveries 包装 * *。 [校准@混沌无形]

Virtual method	方法描述	必须重写？
configure()	当服务器进入on_configure状态时，方法被调用。理想情况下，这个方法应该执行ROS参数的声明和恢复的成员变量的初始化。这个方法需要4个输入参数：父节点的共享指针、恢复名称、TF缓冲区指针和碰撞检查器的共享指针。 [校准@混沌无形]	是 [校准@小鱼]
activate() [待校准@2110]	当恢复服务器进入on_activate状态时，方法被调用。理想情况下，这个方法应该实现恢复到活动状态前的必要操作。 [校准@混沌无形]	是 [校准@小鱼]
deactivate()	当恢复服务器进入on_deactivate状态时，方法被调用。理想情况下，这个方法应该实现恢复进入非活动状态前的必要操作。 [校准@混沌无形]	是 [校准@小鱼]
cleanup()	当恢复服务器进入on_cleanup状态时，方法被调用。理想情况下，这个方法应该清理为恢复所创建的资源。 [校准@混沌无形]	是 [校准@小鱼]

对于提供ROS 2动作接口和模板的 [](http://dev.nav2.fishros.com/doc/plugin_tutorials/docs/writing_new_recovery_plugin.html#id1)nav2_recoveries包装器，我们有4个虚拟方法需要实现。本教程使用这个包装器，所以这些是我们要解决的主要内容。 [校准@混沌无形]

Virtual method	方法描述	必须重写？
onRun() [待校准@2191]	当收到一个新的恢复行动请求时，方法被立即调用。给出要处理的行动目标，并应开始恢复初始化/进程。 [校准@混沌无形]	是 [校准@小鱼]
onCycleUpdate() [待校准@2193]	方法在恢复更新率时被调用，应该完成任何必要的更新。一个旋转的例子是计算当前周期的指令速度，发布它并检查是否完成。 [校准@混沌无形]	是 [校准@小鱼]
Onconfiguration() [待校准@2195]	当恢复服务器进入on_configure状态时，方法被调用。理想情况下，这个方法应该实现恢复进入配置状态前的必要操作（获取参数等）。 [校准@混沌无形]	否 [校准@小鱼]
onCleanup() [待校准@2197]	当恢复服务器进入on_cleanup状态时，方法被调用。理想情况下，这个方法应该清理为恢复所创建的资源。 [校准@混沌无形]	否 [校准@小鱼]

在本教程中，我们将使用onRun() 、 onCycleUpdate() 和 onConfigure()``方法来创建SMS恢复。为了简洁起见，将跳过 ``onConfigure() ，但只包括参数声明。 [校准@混沌无形]

在恢复中，onRun() 方法必须设置任何初始状态并启动恢复行为。对于我们调用帮助恢复行为的情况，我们可以在这个方法中琐碎地计算我们所有的需求。 [校准@混沌无形]

Status SMSRecovery::onRun(const std::shared_ptr 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 Status::FAILED;
  }

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

我们收到一个行动目标， command ，我们想在其中进行处理。 command 包含一个 message 字段，其中包含我们想要传达给我们母舰的信息。这是我们想通过短信发送给行动中心的战友的 “call for help” 信息。 [校准@混沌无形]

我们使用Twilio服务来完成这项任务。请 创建一个账户 和所有相关信息需要创建服务 (例如 account_sid ， auth_token ，电话号码)。你可以在你的配置文件中把这些值设置为对应于onConfigure()参数声明的参数。 [校准@混沌无形]

我们使用_twilio对象来发送我们的消息，其中包括你在配置文件中的账户信息。我们发送消息并在屏幕上记录消息是否发送成功。我们返回 FAILED 或 SUCCEEDED，这取决于返回给行动客户端的值。 [校准@混沌无形]

[](http://dev.nav2.fishros.com/doc/plugin_tutorials/docs/writing_new_recovery_plugin.html#id1)onCycleUpdate()由于我们的短时运行的恢复行为，所以是很简单的。如果恢复反而是更长时间的运行，如旋转，导航到安全区域，或离开一个糟糕的地方并等待帮助，那么这个函数将检查超时或计算控制值。对于我们的例子，我们只是简单地返回成功，因为我们已经在onRun()中完成了我们的任务。 [校准@混沌无形]

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

其余的方法不使用，也不强制覆盖它们。 [校准@混沌无形]

2-导出恢复插件

现在我们已经创建了我们的自定义恢复，我们需要导出我们的恢复插件，这样它就可以在恢复服务器上看到。插件在运行时被加载，如果它们不可见，那么我们的恢复服务器将无法加载它。在ROS 2中，导出和加载插件是由pluginlib处理的。 [校准@混沌无形]

在我们的教程中，“nav2_sms_recovery::SMSRecovery 类被动态加载为 nav2_core:: recovery”，这是我们的基类。 [校准@混沌无形]

1. 导出恢复插件，我们需要提供两行 [校准@混沌无形]

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_sms_recovery::SMSRecovery, nav2_core::Recovery)

请注意，它需要pluginlib导出插件的类。Pluginlib将提供作为宏的 PLUGINLIB_EXPORT_CLASS ，它负责所有的导出工作。 [待校准@2167]

把这些行内容放在文件的末尾是很好的做法，但从技术上讲，你也可以写在顶部。 [校准@混沌无形]

2. 下一步是在包的根目录中创建插件的描述文件。例如，我们的示例软件包中的 recovery_plugin.xml 文件。此文件包含以下信息 [校准@混沌无形]

• 参数library path:插件的库名和它的位置。 [待校准@2134]
• 参数class name:类的名称。 [校准@混沌无形]
• 参数class type:类的类型 [待校准@2136]
• 参数base class:基类的名称。 [待校准@2137]
• 参数description:插件的描述。 [待校准@2138]

  
    This is an example plugin which produces an SMS text message recovery.
  

3. 下一步是使用cmake函数pluginlib_export_plugin_description_file()导出插件``CMakeLists.txt。此函数将插件描述文件安装到 share` 目录，并设置属性索引以使其可被发现。 [校准@混沌无形]

pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml)

4. 插件描述文件也应添加到 package.xml [待校准@2171]

  ament_cmake
  

5. 编译并注册。接下来，我们将使用这个插件。 [待校准@2172]

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

要启用插件，我们需要如下修改 nav2_params.yaml 文件以替换以下参数 [待校准@2210]

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    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

与 [待校准@2175]

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait", "call_for_help"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    call_for_help:
      plugin: "nav2_sms_recovery/SMSRecovery"
      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

在上面文中，您可以观察到我们在 call_for_help ROS 2操作服务器名称下添加了SMS恢复。我们还告诉恢复服务器， call_for_help 是 SMSRecovery 类型的，并为您的Twilio帐户提供我们的参数。 [校准@混沌无形]

4- 运行恢复插件 [校准@混沌无形]

在启用Nav2的情况下运行Turtlebot3的模拟，详细的说明写在 入门 。以下是快捷键命令： [校准@混沌无形]

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

在新的终端运行: [校准@混沌无形]

$ ros2 action send_goal "call_for_help" nav2_sms_recovery/action/SmsRecovery "Help! Robot 42 is being mean :( Tell him to stop!"

• 本文主要校准贡献：混沌无形
• 本文遵循知识共享协议，禁止未授权商用转载