ROS2与Navigation2入门教程-编写新的规划器（Planner）插件

说明：

• 介绍如何编写新的规划器（Planner）插件

概述

• 本教程将会说明如何创建自己的规划器插件

要求

• 要求在本地机器上已经安装或构建好了以下软件包：

  • ROS 2（二进制安装或从源代码构建）
  • Nav2（包括依赖包）
  • Gazebo
  • Turtlebot3

具体步骤

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

• 本教程中将会创建一个简单的直线规划器。

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

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

• 本示例插件继承自基类nav2_core::GlobalPlanner。该基类提供了5个纯虚方法来实现规划器插件。

• 规划器服务器将会使用插件来计算轨迹。

• 下面来了解更多关于编写规划器插件所需的一些方法。

• 在本教程中，将会使用方法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。Navigation2允许加载多个插件并保持组织有序，每个插件会映射到某个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;

• 其余方法没有使用，但必须覆盖它们。

• 根据规则，这里确实会覆盖所有这些方法，但这些方法均为空的。

导出规划器插件

• 现在已经创建了自定义规划器，需要导出该规划器插件以便规划器服务器可以看见该插件。

• 插件在运行时加载，但如果插件不可见，则规划器服务器将无法加载插件。

• 在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文件。

• 此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>

• 编译该插件软件包，这样应该就完成了该插件的注册。下一步就可以使用该插件了。

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

• 要启用该插件，需要如下所示修改nav2_params.yaml文件即替换下列参数：

• 将以下参数：

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

• 用下面的参数替换：

planner_server:
ros__parameters:
  planner_plugin_types: ["nav2_straightline_planner/StraightLine"] # For Eloquent and earlier
  planner_plugin_ids: ["GridBased"] # For Eloquent and earlier
  plugins: ["GridBased"] # For Foxy and later
  use_sim_time: True
  GridBased:
    plugin: nav2_straightline_planner/StraightLine # For Foxy and later
    interpolation_resolution: 0.1

• 对于Galactic或更高版本，plugin_names和plugin_types已替换为插件名称的单个plugins字符串向量。

• 这些类型现在是在plugin:字段的plugin_name命名空间中定义的（如plugin: MyPlugin::Plugin）。

• 代码块中的行内注释将帮助指导您完成此操作。

• 在上面代码段中可以看到将nav2_straightline_planner/StraightLine 规划器映射到其id—GridBased上。

• 为了传递特定于插件的参数，这里使用了<plugin_id>.<plugin_specific_parameter>。

运行StraightLine插件

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

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

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

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

• 然后进入到RViz中并单击顶部的“2D Pose Estimate”按钮，并按照“开始使用Nav2”教程所介绍的那样在地图上点击机器人的初始位置。

• 机器人将会在地图上定位，然后点击“Navigation2 goal”按钮，并在您想要规划器将其视作目标位姿的位置上单击。

• 这样，规划器就会规划该路径，且机器人就会开始朝着目标移动。

参考：

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