## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Writing a local path planner as plugin in ROS 
## multi-line description to be displayed in search 
## description = A tutorial to writing a custom local planner to work with the ROS1.This tutorial will be structured in a similar manner to [[http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS/|ROS Global Path Planner]]
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link= [[http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS|Writing a Global Planner Plugin]]
## next.1.link=
## what level user is this tutorial for 
## level= BeginnerCategory
## keywords = local planner, move_base
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>
{{{#!wiki blue/solid
'''Note:''' The ROS Wiki is generally for ROS 1 only! If you have installed ROS 2 please use the [[https://index.ros.org/doc/ros2/Contributing/Developer-Guide/|ROS 2 documentation website]]. 
}}}

== Writing the Path Planner Class ==
=== ROS Installation ===
Before starting these tutorials please complete installation as described in the [[ROS/Installation|ROS installation instructions]].
{{{}}}


=== Class Implementation Header ===
The first step is to write a new class for the path planner that adheres to the [[https://github.com/ros-planning/navigation/blob/noetic-devel/nav_core/include/nav_core/base_local_planner.h|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:

{{{#!cplusplus
#ifndef LOCAL_PLANNER_H_
#define LOCAL_PLANNER_H_

// abstract class from which our plugin inherits
#include <nav_core/base_local_planner.h>

#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>

using namespace std;

namespace local_planner{

class LocalPlanner : public nav_core::BaseLocalPlanner{
public:

    LocalPlanner();
    LocalPlanner(std::string name, tf2_ros::Buffer* tf,
                 costmap_2d::Costmap2DROS* costmap_ros);

    ~LocalPlanner();

    void initialize(std::string name, tf2_ros::Buffer* tf,
                    costmap_2d::Costmap2DROS* costmap_ros);

    bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);

    bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);

    bool isGoalReached();
private:
    costmap_2d::Costmap2DROS* costmap_ros_;
    tf2_ros::Buffer* tf_;
    bool initialized_;
};
};

#endif
}}}

=== 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:

{{{#!cplusplus
#include "local_planner/local_planner.h"
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(local_planner::LocalPlanner, nav_core::BaseLocalPlanner)

namespace local_planner{

LocalPlanner::LocalPlanner() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {}

LocalPlanner::LocalPlanner(std::string name, tf2_ros::Buffer* tf,
                           costmap_2d::Costmap2DROS* costmap_ros)
    : costmap_ros_(NULL), tf_(NULL), initialized_(false)
{
    initialize(name, tf, costmap_ros);
}

LocalPlanner::~LocalPlanner() {}

// Take note that tf::TransformListener* has been changed to tf2_ros::Buffer* in ROS Noetic
void LocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf,
                              costmap_2d::Costmap2DROS* costmap_ros)
{
    if(!initialized_)
    {
        tf_ = tf;
        costmap_ros_ = costmap_ros;
        initialized_ = true;
    }
}

bool LocalPlanner::setPlan(
    const std::vector<geometry_msgs::PoseStamped>& orig_global_plan
)
{
    if(!initialized_)
    {
        ROS_ERROR("This planner has not been initialized");
	return false;
    }
    return true;
}

bool LocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
    if(!initialized_)
    {
        ROS_ERROR("This planner has not been initialized");
	return false;
    }
    return true;
}

bool LocalPlanner::isGoalReached()
{
    if(!initialized_)
    {
	ROS_ERROR("This planner has not been initialized");
	return false;
    }
    return false;
}
}
}}}
== Writing your Plugin ==
Create a file called lp_plugin.xml but the file can be named just about anything

{{{#!yaml
<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" />
}}}
   


## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE