编写新的规划器插件

• 概述

• 必要条件

• 教程步骤

概述

本教程展示了如何创建自己的规划器插件。 [校准@混沌无形]

必要条件

• ROS 2 (二进制或从源代码构建)

• Nav2 (包括依赖项) [待校准@1996]

• Gazebo [待校准@1997]

• Turtlebot3 [待校准@1998]

教程步骤

1创建新规划器插件

我们将创建一条简单的直线规划器。本教程中的注释代码可以在 navigation_tutorials 库中找到，因为 nav2_straightline_planner 这个包可以被认为是编写规划器插件的参考。 [校准@混沌无形]

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

Virtual method	方法描述	必须重写？
configure()	当规划器服务器进入on_configure状态时，该方法被调用。理想情况下，这个方法应该执行ROS参数的声明和规划器成员变量的初始化。该方法4个输入参数: 指向父节点的共享指针，规划器名称，tf缓冲区指针和指向成本地图的共享指针。 [校准@混沌无形]	是 [校准@小鱼]
activate() [待校准@2110]	当规划器服务器进入on_activate状态时，方法被调用。理想情况下，这个方法应该在规划器进入激活状态之前实现必要的操作。 [校准@混沌无形]	是 [校准@小鱼]
deactivate()	当规划器服务器进入on_deactivate状态时，方法被调用。理想情况下，这个方法应该实现规划器进入非活动状态前的必要操作。 [校准@混沌无形]	是 [校准@小鱼]
cleanup()	当规划器服务器进入on_cleanup状态时，方法被调用。理想情况下，这个方法应该清理为规划器创建的资源。 [校准@混沌无形]	是 [校准@小鱼]
createPlan()	当规划器服务器要求为指定的起始和目标姿势提供全局规划时，该方法被调用。该方法返回携带全局规划的 “nav_msgs::msg:: path”。这个方法需要两个输入参数：起始位姿和目标位姿。 [校准@混沌无形]	是 [校准@小鱼]

对于本教程，我们将使用方法 StraightLine::configure() 和 StraightLine::createPlan() 创建直线规划器。 [待校准@2159]

在规划器中， 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" 并检索 interpolation_resolution 参数，这是特定于"GridBased" 的，我们使用 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-导出planner插件

既然我们已经创建了我们的自定义规划器，我们需要导出我们的规划器插件，以便它对规划器服务器可见。插件在运行时加载，如果它们不可见，那么我们的规划器服务器将无法加载它。在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)

请注意，它需要pluginlib导出插件的类。Pluginlib将提供作为宏的 PLUGINLIB_EXPORT_CLASS ，它负责所有的导出工作。 [待校准@2167]

把这些行内容放在文件的末尾是很好的做法，但从技术上讲，你也可以写在顶部。 [校准@混沌无形]

2. 下一步将是在软件包的根目录下创建插件的描述文件。例如，我们的教程包中的 global_planner_plugin.xml 文件。此文件包含以下信息 [校准@混沌无形]

• 参数``library path``:插件的库名和它的位置。 [待校准@2134]

• 参数``class name``:类的名称。 [校准@混沌无形]

• 参数``class type``:类的类型 [待校准@2136]

• 参数``base class``:基类的名称。 [待校准@2137]

• 参数``description``:插件的描述。 [待校准@2138]

<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. 下一步是使用cmake函数``pluginlib_export_plugin_description_file()``导出插件``CMakeLists.txt`。此函数将插件描述文件安装到 share 目录，并设置属性索引以使其可被发现。 [校准@混沌无形]

pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)

4. 插件描述文件也应添加到 package.xml [待校准@2171]

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

5. 编译并注册。接下来，我们将使用这个插件。 [待校准@2172]

3-通过params文件传递插件名称

要启用插件，我们需要将 nav2_params.yaml 文件修改为以下内容，以替换以下参数 [待校准@2174]

备注

用Galactic或后， plugin_names 和 plugin_types 替换单个 plugins string载体插件名称。类型现在定义在 plugin_name 命名空间 ''plugin: `` field (e.g. `` 插件: MyPlugin: Plugin'')。内联注释代码块将帮助您通过这个。 [待校准@2091]

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

与 [待校准@2175]

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

在上面的片段中，你可以观察到我们的``nav2_straightline_planner/StraightLine``规划器与它的id GridBased``的映射。为了传递插件特定的参数，我们使用了 ``<plugin_id>.<plugin_specific_parameter> 。 [校准@混沌无形]

4-运行直线插件

运行Turtlebot3模拟器，启用navigation 2。如何制作它的详细说明写在 入门 。下面是该命令的快捷方式: [校准@混沌无形]

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

然后转到RViz，点击顶部的 "2D Pose Estimate" 按钮，在地图上指出位置，就像 入门 中描述的那样。机器人将在地图上定位，然后点击 "Navigation2 goal" ，点击你想让你的规划器考虑目标位姿。之后，规划器将规划路径，机器人将开始向目标移动。 [校准@混沌无形]