< >
Home » ROS2与Navigation2入门教程 » ROS2与Navigation2入门教程-编写新的恢复器（Recovery）插件

ROS2与Navigation2入门教程-编写新的恢复器（Recovery）插件

说明：

  • 介绍如何编写新的恢复器（Recovery）插件

概述

  • 本教程将会说明如何创建自己的恢复器插件。恢复器插件位于恢复器服务器中。

  • 与规划器和控制器服务器不同，每个恢复器都会托管自己独特的动作服务器。

  • 规划器和控制器在完成相同的任务时具有相同的API。

  • 但是，恢复器可用于执行多种任务，因此每个恢复器都可以有自己独特的动作消息定义和动作服务器。

  • 这为恢复器服务器提供了巨大的灵活性，可以实现任何可以想象的不需要其他重用的恢复器动作。

要求

  • 要求在本地机器上已经安装或构建好了以下软件包：
    • ROS 2（二进制安装或从源代码构建）
    • Nav2（包括依赖包）
    • Gazebo
    • Turtlebot3

具体步骤

  • 创建一个新的恢复器插件

  • 本教程中将会创建一个简单的呼叫帮助恢复器行为。它会使用 Twilio通过SMS向远程操作中心发送消息。

  • 本教程中的代码可以在navigation_tutorials资源库的nav2_sms_recovery软件包中找到。

  • 这个软件包可以作为编写恢复器插件的参考。

  • 本示例插件实现了nav2_core::Recovery插件类。

  • 但是，在nav2_recoveries中有一个很好的动作封装器，因此会为此应用程序使用nav2_recoveries::Recovery基类。

  • 这个封装类派生自nav2_core类，因此它可以用作插件，但还可以处理所需的绝大多数ROS 2动作服务器样板。

  • 基类nav2_core提供了4个纯虚拟方法来实现恢复器插件。恢复器服务器将会使用该插件来托管插件，但每个插件都会提供自己独特的动作服务器接口。

  • 如果还没有使用过“nav2_recoveries”封装器的话，下面来了解一下关于编写恢复器插件所需方法的更多信息。

虚拟方法方法简介是否要求覆写
configure()当服务器进入on_configure状态时会调用此方法。理想情况下，此方法应该执行ROS参数声明和恢复器成员变量的初始化。此方法需要4个输入参数：指向父节点的共享指针、恢复器名称、tf缓冲区指针和指向碰撞检查器的共享指针。是
activate()当恢复器服务器进入on_activate状态时会调用此方法。理想情况下，此方法应实现恢复器进入活动状态前的必要操作。是
deactivate()当恢复器服务器进入on_deactivate状态时调用此方法。理想情况下，此方法应实现恢复器进入非活动状态前的必要操作。是
cleanup()当恢复器服务器进入on_cleanup状态时会调用此方法。理想情况下，此方法应清除为恢复器创建的各种资源。是

  • 提供ROS 2动作接口和样板的nav2_recoveries封装器有4个要实现的虚拟方法。

  • 本教程会使用此封装器，下表中列出了将要涉及的几个主要方法。

虚拟方法方法简介是否要求覆写
onRun()当收到新的恢复动作请求时会立即调用此方法。此方法会为进程提供动作目标，并应该启动恢复器初始化/进程。是
onCycleUpdate()此方法会以恢复器更新速率被调用，并应该完成任何必要的更新。旋转的一个示例是计算当前周期的指令速度，发布该速度并检查完成情况。是
onConfigure()当恢复器服务器进入on_configure状态时会调用此方法。理想情况下，此方法应该实现恢复器进入配置状态（获取参数等）前的必要动作。否
onCleanup()当恢复器服务器进入on_cleanup状态时会调用此方法。理想情况下，此方法应该清除为恢复器创建的各种资源。否

  • 本教程中将会使用onRun()、onCycleUpdate()和onConfigure()方法来创建SMS恢复器。

  • 为简洁起见，将会跳过onConfigure()方法，而仅包括参数声明。

  • 在恢复器中，onRun()方法必须设置任何初始状态并启动恢复行为。

  • 对于呼叫帮助恢复行为示例，可以在此方法中轻松计算所有需求。

Status SMSRecovery::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 Status::FAILED;
  }

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

  • 这里会接收到一个要在其中进行处理的动作目标即command。

  • command包含一个字段message，该字段中包含想要传达给“母舰”的消息。

  • 这是想通过短信发送给操作中心战友的“求救”信息。

  • 这里会使用服务Twilio来完成这项任务。

  • 请创建一个帐户并获取创建服务所需的所有相关信息（例如account_sid、auth_token和电话号码）。

  • 可以将这些值设置为与onConfigure()参数声明相对应的配置文件中的参数。

  • 这里使用_twilio对象发送消息，该消息包含来自配置文件中的帐户信息。

  • 无论消息是否成功发送，都会发送消息并将日志显示在屏幕上。

  • 取决于返回给动作客户端的值，会返回FAILED或SUCCEEDED。

  • 方法onCycleUpdate()非常简单，其结果就是短时运行的恢复器行为。

  • 如果恢复器运行时间更长，如旋转、导航到某个安全区域或者离开坏点并等待帮助等，则此函数应该要检查超时或计算控制值。

  • 本示例只是简单地返回成功结果，因为已经在onRun()中完成了任务。

Status SMSRecovery::onCycleUpdate()
{
  return Status::SUCCEEDED;
}
  • 本教程中不会使用其余方法，而且也不强制要求覆盖它们。

导出恢复器插件

  • 现在已经创建好了自定义恢复器，需要导出该恢复器插件以便它对恢复器服务器可见。

  • 插件在运行时加载，但如果它们不可见，那么恢复器服务器将无法加载插件。

  • 在ROS 2中，插件的导出和加载由 pluginlib处理。

  • 至于本教程，类nav2_sms_recovery::SMSRecovery动态加载为基类nav2_core::Recovery。

  • 要导出恢复器，需要提供两行代码：

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

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

  • 将这两行代码放在文件末尾是一种很好的做法，但从技术上来说也可以将这两行代码写在文件顶部。

  • 下一步要在该软件包的根目录中创建插件的描述文件。

  • 例如，本教程软件包中的recovery_plugin.xml文件。

  • 该文件包含以下信息：

    • library path：插件库名称及其位置。

    • class name：类的名称。

    • class type：类的类型。

    • base class：基类的名称。

    • description：插件的描述。

<library path="nav2_sms_recovery_plugin">
  <class name="nav2_sms_recovery/SMSRecovery" type="nav2_sms_recovery::SMSRecovery" base_class_type="nav2_core::Recovery">
    <description>This is an example plugin which produces an SMS text message recovery.</description>
  </class>
</library>

  • 下一步要使用软件包根目录下CMakeLists.txt文件中的cmake函数pluginlib_export_plugin_description_file()来导出插件。

  • 此函数会将插件描述文件安装到share目录并设置ament索引以使其可被发现。

pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml)
  • 插件描述文件还应该添加到软件包根目录下的package.xml文件中。
<export>
  <build_type>ament_cmake</build_type>
  <nav2_core plugin="${prefix}/recovery_plugin.xml" />
</export>
  • 编译软件包，这样就注册好了该插件。接下来就可以使用该插件了。

通过参数文件传递插件名称

  • 要启用该插件，需要如下所示修改nav2_params.yaml文件，即需要替换以下参数：
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
  • 替换成下面的参数：
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动作服务器名称下添加了短信恢复器。

  • 该代码段还告知恢复器服务器call_for_help是SMSRecovery类型的，并向其提供了您的Twilio帐户参数。

运行恢复器插件

  • 运行带有已启用Nav2的Turtlebot3机器人仿真。

  • 有关如何启用Nav2的详细说明请参阅“开始使用Nav2”教程。

  • 下面是运行该仿真的快捷命令：

$ 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!"

参考：

  • https://navigation.ros.org/plugin_tutorials/docs/writing_new_recovery_plugin.html

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与navigation2入门教程