编写一个新的规划器插件

概述
要求
教程步骤

概述

本教程展示了如何创建自己的规划器插件。

要求

ROS 2（二进制或源码编译）
Nav2（包括依赖项）
Gazebo
Turtlebot3

教程步骤

1- 创建一个新的规划器插件

我们将创建一个简单的直线规划器。本教程中的注释代码可以在 navigation_tutorials 存储库中找到，作为 nav2_straightline_planner。这个包可以作为编写规划器插件的参考。

我们的示例插件继承自基类 nav2_core::GlobalPlanner。基类提供了 5 个纯虚拟方法来实现规划器插件。该插件将由规划器服务器用于计算轨迹。让我们更多地了解编写规划器插件所需的方法。

虚拟方法	方法描述	需要重写？
configure()	该方法在规划器服务器进入 on_configure 状态时调用。理想情况下，这个方法应该执行ROS参数的声明和规划器成员变量的初始化。该方法接受4个输入参数：父节点的共享指针、规划器名称、tf缓冲指针和成本地图的共享指针。	是
activate()	当规划器服务器进入 on_activate 状态时调用该方法。理想情况下，此方法应该实现在规划器进入活动状态之前必要的操作。	是
deactivate()	该方法在规划器服务器进入 on_deactivate 状态时调用。理想情况下，这个方法应该实现在规划器进入非活动状态之前必要的操作。	是
cleanup()	当规划器服务器进入on_cleanup状态时调用此方法。理想情况下，此方法应清理为规划器创建的资源。	是
createPlan()	当规划器服务器要求指定起始和目标姿态的全局路径时调用此方法。此方法返回携带全局路径的`nav_msgs::msg::Path`。此方法接受两个输入参数：起始姿态和目标姿态。	是

在本教程中，我们将使用方法 StraightLine::configure() 和 StraightLine::createPlan() 来创建直线规划器。

在规划器中，``configure()``方法必须从ROS参数设置成员变量和进行任何所需的初始化。

node_ = parent;
tf_ = tf;
name_ = name;
costmap_ = costmap_ros->getCostmap();
global_frame_ = costmap_ros->getGlobalFrameID();

// Parameter initialization
nav2_util::declare_parameter_if_not_declared(node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1));
node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_);

在这里，name_ + ".interpolation_resolution" 获取了 ROS 参数 interpolation_resolution，该参数是特定于我们的规划器的。Nav2 允许加载多个插件，并为了保持组织，每个插件都映射到某个 ID/名称。现在，如果我们想要检索该特定插件的参数，我们可以像上面的片段中那样使用 <mapped_name_of_plugin>.<name_of_parameter>。例如，我们的示例规划器映射到名称 "GridBased"，为了检索特定于 "GridBased" 的 interpolation_resolution 参数，我们使用了 Gridbased.interpolation_resolution。换句话说，GridBased 被用作插件特定参数的命名空间。当我们讨论参数文件（或参数文件）时，我们将会更多地了解这一点。

在``createPlan()``方法中，我们需要根据给定的起始和目标姿态创建一个路径。使用起始姿态和目标姿态调用``StraightLine::createPlan()``来解决全局路径规划问题。成功后，它将路径转换为``nav_msgs::msg::Path``并返回给规划器服务器。下面的注释显示了此方法的实现。

nav_msgs::msg::Path global_path;

// Checking if the goal and start state is in the global frame
if (start.header.frame_id != global_frame_) {
  RCLCPP_ERROR(
    node_->get_logger(), "Planner will only except start position from %s frame",
    global_frame_.c_str());
  return global_path;
}

if (goal.header.frame_id != global_frame_) {
  RCLCPP_INFO(
    node_->get_logger(), "Planner will only except goal position from %s frame",
    global_frame_.c_str());
  return global_path;
}

global_path.poses.clear();
global_path.header.stamp = node_->now();
global_path.header.frame_id = global_frame_;
// calculating the number of loops for current value of interpolation_resolution_
int total_number_of_loop = std::hypot(
  goal.pose.position.x - start.pose.position.x,
  goal.pose.position.y - start.pose.position.y) /
  interpolation_resolution_;
double x_increment = (goal.pose.position.x - start.pose.position.x) / total_number_of_loop;
double y_increment = (goal.pose.position.y - start.pose.position.y) / total_number_of_loop;

for (int i = 0; i < total_number_of_loop; ++i) {
  geometry_msgs::msg::PoseStamped pose;
  pose.pose.position.x = start.pose.position.x + x_increment * i;
  pose.pose.position.y = start.pose.position.y + y_increment * i;
  pose.pose.position.z = 0.0;
  pose.pose.orientation.x = 0.0;
  pose.pose.orientation.y = 0.0;
  pose.pose.orientation.z = 0.0;
  pose.pose.orientation.w = 1.0;
  pose.header.stamp = node_->now();
  pose.header.frame_id = global_frame_;
  global_path.poses.push_back(pose);
}

global_path.poses.push_back(goal);

return global_path;

剩余的方法没有使用，但是强制要求重写它们。根据规则，我们确实重写了所有方法，但是将它们留空。

2- 导出规划器插件

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

回到我们的教程，类 nav2_straightline_planner::StraightLine 以 nav2_core::GlobalPlanner 的形式进行动态加载，后者是我们的基类。

要导出规划器，我们需要提供两行代码。

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_straightline_planner::StraightLine, nav2_core::GlobalPlanner)

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

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

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

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

class name：类的名称。

class type：类的类型。

base class：基类的名称。

description：插件的描述。

<library path="nav2_straightline_planner_plugin">
  <class name="nav2_straightline_planner/StraightLine" type="nav2_straightline_planner::StraightLine" base_class_type="nav2_core::GlobalPlanner">
    <description>This is an example plugin which produces straight path.</description>
  </class>
</library>

下一步是使用``CMakeLists.txt``通过使用cmake函数``pluginlib_export_plugin_description_file()``导出插件。此函数将插件描述文件安装到``share``目录，并设置ament索引以便发现它。

pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)

插件描述文件还应添加到``package.xml``中。

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

编译后应该注册该插件。接下来，我们将使用这个插件。

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

要启用该插件，我们需要修改``nav2_params.yaml``文件，如下所示，以替换以下参数

注解

对于Galactic或更新版本，plugin_names``和``plugin_types``已经被单个``plugins``字符串向量替代，用于插件名称。现在，类型在``plugin_name``命名空间中的``plugin:``字段中定义（例如：``plugin: MyPlugin::Plugin）。代码块中的内联注释将帮助指导您完成此操作。

planner_server:
  ros__parameters:
    plugins: ["GridBased"]
    use_sim_time: True
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner" # For Foxy and later
      tolerance: 2.0
      use_astar: false
      allow_unknown: true

with

planner_server:
  ros__parameters:
    plugins: ["GridBased"]
    use_sim_time: True
    GridBased:
      plugin: "nav2_straightline_planner/StraightLine"
      interpolation_resolution: 0.1

在上面的代码片段中，您可以观察到我们的``nav2_straightline_planner/StraightLine``规划器与其id``GridBased``的映射关系。为了传递特定于插件的参数，我们使用了``<plugin_id>.<plugin_specific_parameter>``的形式。

4- 运行 StraightLine 插件

使用已启用的navigation2运行Turtlebot3模拟。有关如何执行此操作的详细说明，请参阅:ref:getting_started。下面是一个快捷命令：

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

然后进入 RViz，点击顶部的 "2D Pose Estimate" 按钮，并指向地图上的位置，就像在开始使用中所描述的那样。机器人将在地图上进行定位，然后点击 "Navigation2 goal"，并点击您希望规划器考虑的目标位姿。之后规划器将规划路径，机器人将开始朝目标前进。