Nav2插件教程（没完

zzqcays

已于 2023-12-04 23:42:12 修改

阅读量166

点赞数

文章标签：前端 javascript 开发语言

于 2023-12-04 21:08:47 首次发布

本文链接：https://blog.csdn.net/weixin_53805189/article/details/134770764

版权

Writing a New Costmap2D Plugin编写新的 Costmap2D 插件

Overview

这个教程展示了如何为Costmap2D创建你自己的简单 plugin 。

在开始教程之前，请查看这个 video ，其中包含有关Costmap2D层设计和插件基本操作原则的信息。

Requirements

假设 ROS 2、Gazebo 和 TurtleBot3 包已在本地安装或构建。请确保 Nav2 项目也像在“ Build and Install ”中那样在本地构建。

Tutorial Steps

1- Write a new Costmap2D plugin

这个示例将创建一个成本地图插件，用于在成本地图中创建重复的成本梯度。本教程的注释代码可以在 navigation2_tutorials 仓库中的 ROS 2 包中找到。在制作自己的 Costmap2D 层插件时，请参考此处。

插件类 nav2_gradient_costmap_plugin::GradientLayer 继承自基础类： nav2_costmap_2d::Layer。

namespace nav2_gradient_costmap_plugin
{

class GradientLayer : public nav2_costmap_2d::Layer
#这段代码是在命名空间 `nav2_gradient_costmap_plugin` 中定义了一个名为 `GradientLayer` 的类。
#这个类继承了 `nav2_costmap_2d` 命名空间中的 `Layer` 类。

基本类提供了一组用于在插件中处理成本地图层的虚拟方法 API。这些方法在运行时被调用。下表列出了方法、它们的描述以及在插件代码中必须具有这些方法的必要性。LayeredCostmap

在我们的示例中，这些方法具有以下功能：

1、GradientLayer::onInitialize() 包含 ROS 参数的声明，以及它的默认值：

declareParameter("enabled", rclcpp::ParameterValue(true));  # 声明一个名为 "enabled" 的参数，并设置默认值为 true
node_->get_parameter(name_ + "." + "enabled", enabled_);  # 获取参数名为 "enabled" 的值并存储在变量 enabled_ 中

这段代码设置了需要重新计算的边界指示器 need_recalculation_。

need_recalculation_ = false;

2、GradientLayer::updateBounds() 在 need_recalculation_ 为 true 时重新计算窗口边界，并无论 need_recalculation_ 的值如何都会更新它们。

3、GradientLayer::updateCosts() - 在这个方法中，梯度直接写入了结果成本地图 master_grid，而没有与之前的图层合并。这相当于在内部 costmap_ 上工作，然后调用

（

GradientLayer::onInitialize()函数初始化了一个 ROS 参数，并且设置了 need_recalculation_ 标志为 false，表示不需要重新计算边界。

GradientLayer::updateBounds()函数会根据 need_recalculation_ 的值重新计算窗口边界，无论该标志的值如何都会更新这些边界。

GradientLayer::updateCosts()方法是关键的方法，它直接将梯度写入到最终的代价地图 master_grid 中，而不是与之前的图层合并。这段代码描述了如何生成主要代价地图的梯度。GRADIENT_SIZE 是地图单元中每个梯度周期的大小，而 GRADIENT_FACTOR 是每步减少的代价地图值。循环遍历了指定区域，为每个单元格设置了相应的代价值，形成了一个梯度的效果，使得代价值递减，模拟了一种渐变的效果。

）

updateWithTrueOverwrite() 方法。以下是用于主成本地图的梯度生成算法：

int gradient_index;
for (int j = min_j; j < max_j; j++) {
  // Reset gradient_index each time when reaching the end of re-calculated window
  // by OY axis.
  gradient_index = 0;
  for (int i = min_i; i < max_i; i++) {
    int index = master_grid.getIndex(i, j);
    // setting the gradient cost
    unsigned char cost = (LETHAL_OBSTACLE - gradient_index*GRADIENT_FACTOR)%255;
    if (gradient_index <= GRADIENT_SIZE) {
      gradient_index++;
    } else {
      gradient_index = 0;
    }
    master_array[index] = cost;
  }
}

GRADIENT_SIZE 是地图单元中每个梯度周期的大小，GRADIENT_FACTOR 是每步减少的成本地图值：

这些参数在插件的头文件中进行了定义。

4、`GradientLayer::onFootprintChanged()` 方法只是重置了 `need_recalculation_` 的值。

5、`GradientLayer::reset()` 方法是虚函数，在这个示例插件中未被使用。它仅保留在这里，因为父类 Layer 中的纯虚函数 `reset()` 需要被重写。

（

GradientLayer::onInitialize(): 这里声明了一个 ROS 参数，并设置了其默认值。接着，将 need_recalculation_ 设置为 false，表示无需重新计算边界。
GradientLayer::updateBounds(): 当 need_recalculation_ 为 true 时，重新计算窗口边界并无论 need_recalculation_ 的值如何都进行更新。
GradientLayer::updateCosts(): 在这个方法中，梯度直接写入了最终的代价地图 master_grid，而不与之前的图层合并。这相当于直接操作内部的 costmap_ 然后调用 updateWithTrueOverwrite() 方法。代码块描述了生成主代价地图的梯度算法。
GradientLayer::onFootprintChanged(): 重新设置了 need_recalculation_ 的值。
GradientLayer::reset(): 这个方法是一个虚拟方法，但在这个示例插件中没有被使用。它保留在此处是因为父类 Layer 中的纯虚拟函数 reset() 要求被覆盖实现。

）

2- Export and make GradientLayer plugin导出并创建 GradientLayer 插件

编写的插件将在运行时作为其基本父类加载，然后会被插件处理模块（对于 costmap2d，是 LayeredCostmap）调用。Pluginlib 在运行时打开给定的插件，并提供从导出类中可调用的方法。类导出机制告诉 pluginlib 应该使用哪个基本类进行这些调用。这允许通过插件扩展应用程序，而无需了解应用程序源代码或重新编译它。

在我们的示例中，`nav2_gradient_costmap_plugin::GradientLayer` 插件的类应该动态加载为 `nav2_costmap_2d::Layer` 基本类。为此，插件应该按以下方式进行注册：

1、插件的类应该使用所加载类的基本类型进行注册。为此，需要在组成插件库的任何源文件中添加特殊的宏 `PLUGINLIB_EXPORT_CLASS`。

#include "pluginlib/class_list_macros.hpp"  # 导入插件宏
PLUGINLIB_EXPORT_CLASS(nav2_gradient_costmap_plugin::GradientLayer, nav2_costmap_2d::Layer)  # 导出 GradientLayer 类作为 Layer 类

Writing a New Planner Plugin编写新的 Planner 插件

Writing a New Controller Plugin编写新的 Controller 插件

Writing a New Behavior Tree Plugin编写新的 Behavior Tree 插件

Overview

这个教程展示了如何创建自己的行为树（BT）插件。BT 插件被用作行为树 XML 中的节点，由 BT 导航器用于导航逻辑。

Requirements

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

Tutorial Steps

1- Creating a new BT Plugin

我们将创建一个简单的BT插件节点来执行另一个服务器上的操作。在这个例子中，我们将分析nav2_behavior_tree包中最简单的行为树动作节点，即等待节点。除了这个动作BT节点的例子外，您还可以创建自定义的装饰器、条件和控制节点。每种节点类型在行为树中都有独特的角色，用于执行诸如规划、控制BT流程、检查条件状态或修改其他BT节点的输出等操作。

本教程中的代码可以在nav2_behavior_tree包中的wait_action节点中找到。这个动作节点可以被视为编写其他动作节点插件的参考。

我们的示例插件继承自基类nav2_behavior_tree::BtActionNode。这个基类是BehaviorTree.CPP BT::ActionNodeBase的封装，简化了使用ROS 2动作客户端的BT动作节点。BTActionNode既是BT动作，又使用ROS 2动作网络接口来调用远程服务器执行一些工作。

当使用其他类型的BT节点（如装饰器、控制、条件）时，请使用相应的BT节点，如BT::DecoratorNode、BT::ControlNode或BT::ConditionNode。对于不使用ROS 2动作接口的BT动作节点，请直接使用BT::ActionNodeBase基类本身

（对于不使用ROS 2动作接口的BT动作节点，指的是那些不需要使用ROS 2中的动作通信机制来执行远程服务器任务的行为树动作节点。相反，它们可能执行本地任务、执行基于话题或服务的通信，或者以其他方式执行无需ROS 2动作通信的操作。BT::ActionNodeBase是BehaviorTree.CPP中的基类，用于这样的节点）。

BTActionNode类提供了5个虚拟方法供使用，除了构造函数中提供的信息。让我们更深入地了解编写BT动作插件所需的方法。

对于本教程，我们只会使用 on_tick() 方法。

在构造函数中，我们需要获取适用于行为树节点的任何非变量参数。在本示例中，我们需要从行为树 XML 的输入端口获取睡眠持续时间的值。

WaitAction::WaitAction(  // 构造函数，初始化等待行为节点
  const std::string & xml_tag_name,  // XML 标签名称
  const std::string & action_name,  // 动作名称
  const BT::NodeConfiguration & conf)  // 节点配置信息
: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)  // 调用 BtActionNode 的构造函数并进行初始化
{
  int duration;  // 时间间隔
  getInput("wait_duration", duration);  // 获取输入参数 wait_duration

  if (duration <= 0) {  // 如果时间间隔小于等于 0
    RCLCPP_WARN(  // 打印警告信息
      node_->get_logger(), "Wait duration is negative or zero "
      "(%i). Setting to positive.", duration);
    duration *= -1;  // 设置为正数
  }

  goal_.time.sec = duration;  // 设置目标的时间间隔
}

这里，我们提供了 xml_tag_name 的输入，它告诉 BT 节点插件与 XML 中对应的字符串。当我们将此 BT 节点注册为插件时，稍后会看到这一点。它还接受将要调用以执行某些行为的动作服务器的字符串名称。最后，一组配置，对于大多数节点插件来说，我们可以安全地忽略。

然后我们调用了 BTActionNode 构造函数。正如所示，它是由 ROS 2 动作类型模板化的，因此我们将其赋予 nav2_msgs::action::Wait 动作消息类型并转发其他输入。BTActionNode 具有 tick() 方法，当此节点从树中调用时，行为树直接调用该方法。在发送动作客户端目标之前，会调用 on_tick()。

在构造函数的主体中，我们通过 getInput 方法获取参数 wait_duration 的输入端口，该参数可以独立配置每个 wait 节点实例的等待时间。它被设置在 duration 参数中，并插入到 goal_ 中。goal_ 类变量是 ROS 2 动作客户端将发送到动作服务器的目标。因此，在此示例中，我们将持续时间设置为所需等待的时间，以便动作服务器了解我们请求的具体细节。

providedPorts() 方法为我们提供了定义输入或输出端口的机会。端口可以被视为行为树节点从行为树本身访问的参数。对于我们的示例，只有一个输入端口，即 wait_duration，可以在 BT XML 中为每个 wait 恢复的实例设置。我们设置了类型为 int，默认值为 1，名称为 wait_duration，以及端口描述“等待时间”。