Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a local path planner as plugin in ROS

Description: A tutorial to writing a custom local planner to work with the ROS1.This tutorial will be structured in a similar manner to ROS Global Path Planner

Keywords: local planner, move_base

Tutorial Level: BEGINNER

Next Tutorial: Writing a Global Planner Plugin

Contents

Writing the Path Planner Class
Writing your Plugin
1. Plugin Registration
2. Usage in move_base

Note: The ROS Wiki is generally for ROS 1 only! If you have installed ROS 2 please use the ROS 2 documentation website.

Writing the Path Planner Class

ROS Installation

Before starting these tutorials please complete installation as described in the ROS installation instructions.

Class Implementation Header

The first step is to write a new class for the path planner that adheres to the ROS Base Local Planner nav_core::BaseLocalPlanner class. For this, you need to create a header file, that we will call in our case, local_planner.h. I will present just the minimal code for adding a plugin, which are the necessary and common steps to add any local planner. Be sure to create a ros package and place the code within the package. The minimal header file is defined as follows:

   1 #ifndef LOCAL_PLANNER_H_
   2 #define LOCAL_PLANNER_H_
   3 
   4 // abstract class from which our plugin inherits
   5 #include <nav_core/base_local_planner.h>
   6 
   7 #include <ros/ros.h>
   8 #include <tf2_ros/buffer.h>
   9 #include <costmap_2d/costmap_2d_ros.h>
  10 #include <geometry_msgs/PoseStamped.h>
  11 #include <geometry_msgs/Twist.h>
  12 
  13 using namespace std;
  14 
  15 namespace local_planner{
  16 
  17 class LocalPlanner : public nav_core::BaseLocalPlanner{
  18 public:
  19 
  20     LocalPlanner();
  21     LocalPlanner(std::string name, tf2_ros::Buffer* tf,
  22                  costmap_2d::Costmap2DROS* costmap_ros);
  23 
  24     ~LocalPlanner();
  25 
  26     void initialize(std::string name, tf2_ros::Buffer* tf,
  27                     costmap_2d::Costmap2DROS* costmap_ros);
  28 
  29     bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
  30 
  31     bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
  32 
  33     bool isGoalReached();
  34 private:
  35     costmap_2d::Costmap2DROS* costmap_ros_;
  36     tf2_ros::Buffer* tf_;
  37     bool initialized_;
  38 };
  39 };
  40 
  41 #endif
  42

Class Implementation Source

For this, you need to create a source file, that we will call in our case, local_planner.cpp with the following:

   1 #include "local_planner/local_planner.h"
   2 #include <pluginlib/class_list_macros.h>
   3 
   4 PLUGINLIB_EXPORT_CLASS(local_planner::LocalPlanner, nav_core::BaseLocalPlanner)
   5 
   6 namespace local_planner{
   7 
   8 LocalPlanner::LocalPlanner() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {}
   9 
  10 LocalPlanner::LocalPlanner(std::string name, tf2_ros::Buffer* tf,
  11                            costmap_2d::Costmap2DROS* costmap_ros)
  12     : costmap_ros_(NULL), tf_(NULL), initialized_(false)
  13 {
  14     initialize(name, tf, costmap_ros);
  15 }
  16 
  17 LocalPlanner::~LocalPlanner() {}
  18 
  19 // Take note that tf::TransformListener* has been changed to tf2_ros::Buffer* in ROS Noetic
  20 void LocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf,
  21                               costmap_2d::Costmap2DROS* costmap_ros)
  22 {
  23     if(!initialized_)
  24     {
  25         tf_ = tf;
  26         costmap_ros_ = costmap_ros;
  27         initialized_ = true;
  28     }
  29 }
  30 
  31 bool LocalPlanner::setPlan(
  32     const std::vector<geometry_msgs::PoseStamped>& orig_global_plan
  33 )
  34 {
  35     if(!initialized_)
  36     {
  37         ROS_ERROR("This planner has not been initialized");
  38         return false;
  39     }
  40     return true;
  41 }
  42 
  43 bool LocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
  44 {
  45     if(!initialized_)
  46     {
  47         ROS_ERROR("This planner has not been initialized");
  48         return false;
  49     }
  50     return true;
  51 }
  52 
  53 bool LocalPlanner::isGoalReached()
  54 {
  55     if(!initialized_)
  56     {
  57         ROS_ERROR("This planner has not been initialized");
  58         return false;
  59     }
  60     return false;
  61 }
  62 }

Writing your Plugin

Create a file called lp_plugin.xml but the file can be named just about anything

<library path="lib/lib_local_planner">
        <class name ="local_planner/LocalPlanner" type ="local_planner::LocalPlanner" base_class_type= "nav_core::BaseLocalPlanner">
                <description>
                        Simplest Plugin for BaseLocalPlanner
                </description>
        </class>
</library>

Plugin Registration

In the package.xml of this package, export the file created in the previous section

  <export>
    <nav_core plugin="${prefix}/lp_plugin.xml"/>
  </export>

Use the following command and type it into the terminal. Your new local planner should now be registered. Take note that this happens after catkin build or catkin_make

rospack plugins --attrib=plugin nav_core

Usage in move_base

Add the following parameter to move_base to use the plugin that was just created. Congrats on creating your very own local planner!

 <param name="base_local_planner" value="local_planner/LocalPlanner" />

ROS 2 Documentation

Wiki

Page

User