navigation2 Write planner plug-in

Article directory

  • navigation2 Write planner plug-in
    • 1- Create a new planner plugin
    • 2- Export planner plugin
      • 1. To export the planner we need to provide two lines of code:
      • 2. The next step is to create the plugin’s description file in the root directory of the package. For example, create the `global_planner_plugin.xml` file in our tutorial package. The file contains the following information:
      • 3. The next step is to export the plugin using CMakeLists.txt, by using the cmake function `pluginlib_export_plugin_description_file()`. This function installs the plugin description file into the share directory and sets the ament index to make it discoverable.
      • 4. The plug-in description file should also be added to the package.xml file.
      • 5. After compilation, the plugin should be registered. Next, we will use this plugin
    • 3- Pass the plugin name via parameter file.
    • 4- Run the StraightLine plugin

navigation2 writing planner plug-in

Tutorial steps are as follows

1- Create a new planner plugin

We will create a simple straight line planner. The annotated code for this tutorial can be found in nav2_straightline_planner in the navigation_tutorials repository. This package can be used as a reference for writing planner plugins.
Our example plugin inherits from the base class nav2_core::GlobalPlanner. The base class provides 5 pure virtual methods to implement the planner plug-in. This plugin will be used by the planner server to calculate paths. Let’s take a deeper look at the methods required to write a planner plugin.

Virtual method Method description Does it need to be rewritten?
configure() This method is called when the planner server enters the on_configure state. Ideally, this method should perform the declaration of ROS parameters and initialization of planner member variables. This method accepts 4 input parameters: a shared pointer to the parent node, the planner name, a tf buffer pointer, and a shared pointer to the cost map. Yes
activate() This method is called when the planner server enters the on_activate state. Ideally, this method should implement the necessary actions before the planner becomes active. Yes
deactivate() This method is called when the planner server enters the on_deactivate state. Ideally, this method should implement the necessary actions before the planner enters the inactive state. Yes
cleanup() This method is called when the planner server enters the on_cleanup state. Ideally, this method should clean up the resources created for the planner. Yes
createPlan() Called when the planner server requests a global plan for the specified start and target poses this method. This method returns nav_msgs::msg::Path carrying the global plan. This method accepts 2 input parameters: starting pose and target pose. Yes

In this tutorial, we will use the StraightLine::configure() and StraightLine::createPlan() methods to create a straight line planner.
In the planner, the configure() method must set member variables from the ROS parameters and perform any required initialization.

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_);

Here, name_ + ".interpolation_resolution" is to get the ROS parameter interpolation_resolution specific to our planner. Nav2 allows multiple plugins to be loaded, and to keep the organization clean, each plugin is mapped to an ID/name. Now, if we want to retrieve the parameters of a specific plugin, we use . just like we did in the above code snippet. For example, our example planner maps to the name “GridBased”, to retrieve the interpolation_resolution parameter specific to “GridBased” we use GridBased.interpolation_resolution . In other words, GridBased is used as the namespace for plugin-specific parameters. We’ll cover this in more detail when discussing parameter files (or parameter files).
In the createPlan() method, we need to create a path based on the given start and target poses. Use the starting posture and target posture to call StraightLine::createPlan() to solve the global path planning problem. If successful, it will convert the path to nav_msgs::msg::Path and return it to the planner server. The comments below show the implementation of this method.

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;

The remaining methods are not used but must be rewritten. As per the rules we do override all these methods but leave them empty.

2- Export planner plug-in

Now that we have created our custom planner, we need to export our planner plugin so that it is visible to the planner server. Plugins are loaded at runtime, if they are not visible then our planner server will not be able to load it. In ROS 2, exporting and loading plugins is handled by pluginlib.
Going back to our tutorial, the nav2_straightline_planner::StraightLine class is dynamically loaded as our base class nav2_core::GlobalPlanner.

1. To export the planner, we need to provide two lines of code:

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

It’s good practice to put these two lines of code at the end of the file, but technically you could also put them at the top of the file.

2. The next step is to create the plugin’s description file in the root directory of the package. For example, create the global_planner_plugin.xml file in our tutorial package. The file contains the following information:

  • library path: The name and location of the plug-in library.
  • class name: The name of the class.
  • class type: The type of class.
  • base class: The name of the base class.
  • description: Description of the plug-in.
<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. The next step is to export the plugin using CMakeLists.txt, by using the cmake function pluginlib_export_plugin_description_file(). This function installs the plugin description file into the share directory and sets the ament index to make it discoverable. 

pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)

4. The plug-in description file should also be added to the package.xml file. 

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

5. After compilation, the plugin should be registered. Next, we will use this plugin

3- Pass the plugin name via a parameter file.

In order to enable the plugin, we need to modify the nav2_params.yaml file as shown below, replacing the following parameters:

  • Remark:
    Starting with the Galactic version, plugin_names and plugin_types have been replaced with a plugins string vector for plugin names. Types are now defined in the plugin field in the plugin_name namespace (for example, plugin: MyPlugin::Plugin). Inline comments within code blocks will help guide you along the way.
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

In the above code snippet, you can observe that we map the nav2_straightline_planner/StraightLine planner to its id GridBased. To pass plugin-specific parameters, we use the format ..

4- Run the StraightLine plug-in

Using Turtlebot3 emulation with Navigation2 enabled. Detailed instructions on how to make it have been written in the Getting Started Guide. Here is a shortcut command:

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

Then go into RViz, click the “2D Pose Estimation” button at the top, and point to the location on the map, just like described in the “Getting Started Guide”. The robot will be positioned on the map, then click on “Navigation2 Target” and click on the target pose you want the planner to consider. After that, the planner will plan the path and the robot will start moving towards the goal.