navigation2 编写规划器（planner）插件

navigation2 编写规划器（planner）插件

教程步骤如下

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。此方法接受2个输入参数：起始姿态和目标姿态。	是

  在本教程中，我们将使用 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。

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

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

将这两行代码放在文件末尾是一个良好的实践，但从技术上讲，你也可以将其放在文件的顶部。

2. 接下来的步骤是在包的根目录中创建插件的描述文件。例如，在我们的教程包中创建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>

3. 接下来的步骤是使用 CMakeLists.txt 导出插件，通过使用 cmake 函数pluginlib_export_plugin_description_file()。这个函数将插件描述文件安装到 share目录，并设置ament索引以使其可被发现。

pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)

4. 插件描述文件也应该添加到package.xml文件中。

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

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

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仿真。如何制作的详细说明已在“入门指南”中编写。以下是一个快捷命令：

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

  然后进入RViz，点击顶部的“2D姿态估计”按钮，并在地图上指向位置，就像在“入门指南”中描述的那样。机器人将在地图上进行定位，然后点击“Navigation2目标”，并单击您希望规划器考虑的目标姿态。之后，规划器将规划路径，机器人将开始朝着目标移动。