编写新的 Planner 插件

概述
要求
教程步骤

概述

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

要求

ROS 2 (二进制或从源代码构建)
Nav2 (包括依赖项)
Gazebo
Turtlebot3

教程步骤

1- 创建一个新的 Planner 插件

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

我们的示例插件继承自基类“nav2_core::GlobalPlanner”。基类提供了 5 种纯虚拟方法来实现规划器插件。规划器服务器将使用该插件来计算轨迹。让我们进一步了解编写规划器插件所需的方法。

Virtual method	Method description	Requires override?
configure()	Method is called at when planner server enters on_configure state. Ideally this methods should perform declarations of ROS parameters and initialization of planner’s member variables. This method takes 4 input params: shared pointer to parent node, planner name, tf buffer pointer and shared pointer to costmap.	Yes
activate()	Method is called when planner server enters on_activate state. Ideally this method should implement operations which are necessary before planner goes to an active state.	Yes
deactivate()	Method is called when planner server enters on_deactivate state. Ideally this method should implement operations which are necessary before planner goes to an inactive state.	Yes
cleanup()	Method is called when planner server goes to on_cleanup state. Ideally this method should clean up resources which are created for the planner.	Yes
createPlan()	Method is called when planner server demands a global plan for specified start and goal pose. This method returns nav_msgs::msg::Path carrying global plan. This method takes 2 input params: start pose and goal pose.	Yes

在本教程中，我们将使用方法“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 用作插件特定参数的命名空间。当我们讨论参数文件（或 params 文件）时，我们将对此有更多了解。

在 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: Plugin’s library name and its 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_straightline_planner_plugin">
  <class type="nav2_straightline_planner::StraightLine" base_class_type="nav2_core::GlobalPlanner">
    <description>This is an example plugin which produces straight path.</description>
  </class>
</library>

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

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-通过 params 文件传递插件名称

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

Note

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

planner_server:
  ros__parameters:
    plugins: ["GridBased"]
    use_sim_time: True
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner" # For Foxy and later. In Iron and older versions, "/" was used instead of "::"
      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 模拟。详细的操作说明写在入门指南。以下是快捷命令：

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

然后转到 RViz，单击顶部的“2D 姿势估计”按钮，并指向地图上的位置，如入门指南中所述。机器人将在地图上定位，然后单击“Navigation2 目标”，然后单击您希望规划器考虑目标姿势的姿势。之后，规划器将规划路径，机器人将开始朝目标移动。