## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = This tutorial was developed for [[hydro|ROS Hydro]] version. However, it is expected to also work with [[groovy|Groovy]] (not tested). For any suggestions/comments about this tutorial, please send an email to Anis Koubaa (akoubaa@coins-lab.org). More information about Global Planner for ROS can be found in the [[http://www.iroboapp.org|iroboapp project page]]. A mirror of this tutorial is also available in [[http://www.iroboapp.org/index.php?title=Adding_A_Global_Path_Planner_As_Plugin_in_ROS#Running_the_Plugin_on_the_Turtlebot|this link]]. ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= [[ROS/Tutorials|ROS tutorials]] ## note.1= [[Robots/TurtleBot|Turtlebot]] ## note.2= [[http://wiki.ros.org/navigation/Tutorials/Writing%20a%20Local%20Path%20Planner%20As%20Plugin%20in%20ROS|Writing a Local Planner Plugin]] ## descriptive title for the tutorial ## title = Writing A Global Path Planner As Plugin in ROS ## multi-line description to be displayed in search ## description = In this tutorial, I will present the steps for writing and using a global path planner in ROS. The first thing to know is that to add a new global path planner to ROS, the new path planner must adhere to the `nav_core::BaseGlobalPlanner` C++ interface defined in [[nav_core|nav_core]] package. Once the global path planner is written, it must be added as a plugin to ROS so that it can be used by the [[move_base|move_base]] package. In this tutorial, I will provide all the steps starting from writing the path planner class until deploying it as a plugin. I will use Turtlebot as an example of robot to deploy the new path planner. For a tutorial that shows how to integrate a real GA planner as ROS plugin, refer to [[http://www.iroboapp.org/index.php?title=Adding_Genetic_Algorithm_Global_Path_Planner_As_Plugin_in_ROS|Adding Genetic Algorithm Global Path Planner As Plugin in ROS]]. A video tutorial is also available in [[https://www.youtube.com/watch?v=We1gGDXAO_o&list=PL93n88K6Qpb63pyaaPaOTudQB30Z2Qn4r|this link]] ## next.0.link= ## next.1.link= ## what level user is this tutorial for ## level= IntermediateCategory ## keywords = Global Planner, Navigation Stack, Turtlebot, nav_core, Costmap #################################### <> <> == Writing the Path Planner Class == === Class Header === The first step is to write a new class for the path planner that adheres to the [[http://docs.ros.org/api/nav_core/html/classnav__core_1_1BaseGlobalPlanner.html|nav_core::BaseGlobalPlanner]]. A similar example can be found in the [[http://docs.ros.org/hydro/api/carrot_planner/html/carrot__planner_8h_source.html|carrot_planner.h]] as a reference. For this, you need to create a header file, that we will call in our case, global_planner.h. I will present just the minimal code for adding a plugin, which are the necessary and common steps to add any global planner. The minimal header file is defined as follows: {{{#!cplusplus /** include the libraries you need in your planner here */ /** for global path planner interface */ #include #include #include #include #include #include #include #include using std::string; #ifndef GLOBAL_PLANNER_CPP #define GLOBAL_PLANNER_CPP namespace global_planner { class GlobalPlanner : public nav_core::BaseGlobalPlanner { public: GlobalPlanner(); GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); /** overridden classes from interface nav_core::BaseGlobalPlanner **/ void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan ); }; }; #endif }}} Now, I will explain the different parts of the header file. {{{#!cplusplus #include #include #include #include #include #include #include #include }}} It is necessary to include core ROS libraries needed for path planner. [[http://wiki.ros.org/costmap_2d|]] and [[http://wiki.ros.org/costmap_2d|]] are needed to use the [[http://docs.ros.org/hydro/api/costmap_2d/html/classcostmap__2d_1_1Costmap2D.html|costmap_2d::Costmap2D]] class that will be used by the path planner as input map. This map will be accessed automatically by the path planner class when defined as plugin. There is no need to subscribe to `costmap2d` to get the cost map from ROS. [[http://docs.ros.org/hydro/api/nav_core/html/base__global__planner_8h.html|]] is used to import the interface [[http://docs.ros.org/hydro/api/nav_core/html/classnav__core_1_1BaseGlobalPlanner.html|nav_core::BaseGlobalPlanner]], which the plugin must adhere to. {{{#!cplusplus namespace global_planner { class GlobalPlanner : public nav_core::BaseGlobalPlanner { }}} It is a good practice, although not necessary, to define namespace for your class. Here, we define the namespace as `global_planner` for the class `GlobalPlanner`. The namespace is used to define a full reference to the class, as `global_planner::GlobalPlanner`. The class `GlobalPlanner` is then defined and inherits from the interface [[http://docs.ros.org/hydro/api/nav_core/html/classnav__core_1_1BaseGlobalPlanner.html|nav_core::BaseGlobalPlanner]]. All methods defined in [[http://docs.ros.org/hydro/api/nav_core/html/classnav__core_1_1BaseGlobalPlanner.html|nav_core::BaseGlobalPlanner]] must be overridden by the new class `GlobalPlanner`. {{{#!cplusplus public: GlobalPlanner(); GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); /** overriden classes from interface nav_core::BaseGlobalPlanner **/ void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan ); }}} The constructor `GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)` is used to initialize the costmap, that is the map that will be used for planning (`costmap_ros`), and the name of the planner (`name`). The same for the default constructor `GlobalPlanner()` which initializes the planner attributes with default values. The method `initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)` is an initialization function for the `BaseGlobalPlanner`, which initializes the costmap, that is the map that will be used for planning (`costmap_ros`), and the name of the planner (`name`). For the particular case of the [[http://docs.ros.org/hydro/api/carrot_planner/html/carrot__planner_8cpp_source.html|carrot_planner]], the `initialize` method is implemented as follows: {{{#!cplusplus void CarrotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){ if(!initialized_){ costmap_ros_ = costmap_ros; //initialize the costmap_ros_ attribute to the parameter. costmap_ = costmap_ros_->getCostmap(); //get the costmap_ from costmap_ros_ // initialize other planner parameters ros::NodeHandle private_nh("~/" + name); private_nh.param("step_size", step_size_, costmap_->getResolution()); private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10); world_model_ = new base_local_planner::CostmapModel(*costmap_); initialized_ = true; } else ROS_WARN("This planner has already been initialized... doing nothing"); } }}} Then, the method `bool makePlan(start, goal, plan)` must be overridden. The final plan will be stored in the parameter `std::vector& plan` of the method. This plan will be automatically published through the plugin as a topic. An implementation of the makePlan method of the carrot_planner can be found [[http://docs.ros.org/hydro/api/carrot_planner/html/carrot__planner_8cpp_source.html|in this link]] as a reference. === Class Implementation === In what follows, I present the main issues to be considered in the implementation of a global planner as plugin. I will not describe a complete path planning algorithm. I will use a dummy example of path planning for makePlan() method just for illustration purposes (''this is can be improved in the future''). Here is a minimum code implementation of the global planner (`global_planner.cpp`), which always generates a dummy static plan. {{{#!cplusplus #include #include "global_planner.h" //register this planner as a BaseGlobalPlanner plugin PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner) using namespace std; //Default Constructor namespace global_planner { GlobalPlanner::GlobalPlanner (){ } GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){ initialize(name, costmap_ros); } void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){ } bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector& plan ){ plan.push_back(start); for (int i=0; i<20; i++){ geometry_msgs::PoseStamped new_goal = goal; tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54); new_goal.pose.position.x = -2.5+(0.05*i); new_goal.pose.position.y = -3.5+(0.05*i); new_goal.pose.orientation.x = goal_quat.x(); new_goal.pose.orientation.y = goal_quat.y(); new_goal.pose.orientation.z = goal_quat.z(); new_goal.pose.orientation.w = goal_quat.w(); plan.push_back(new_goal); } plan.push_back(goal); return true; } }; }}} The constructors can be implemented with respect to the planner requirements and specification. Their implementation is not considered in this particular illustrative example. There are few ''important'' things to consider: * '''Register the planner as BaseGlobalPlanner plugin''':this is done through the instruction `PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)`. For this it is necessary to include the library `#include ` * '''The implementation of the `makePlan()` method''': The `start` and `goal` parameters are used to get initial location and target location, respectively. In this illustrative implementation, the plan vector is initiated with the start location (`plan.push_back(start)`). Then, 20 new dummy locations will be statically inserted in the plan in the`for` loop, then, the `goal` location is inserted in the plan as last location. This planned path will then be sent to the [[move_base]] global planner module which will publish it through the ROS topic `nav_msgs/Path`, which will then be received by the local planner module. Now that your global planner class is done, you are ready for the second step, that is creating the plugin for the global planner to integrate it in the global planner module [[http://docs.ros.org/hydro/api/nav_core/html/classnav__core_1_1BaseGlobalPlanner.html|nav_core::BaseGlobalPlanner]] of the [[move_base]] package. === Compilation === To compile the global planner library created above, it must be added to the `CMakeLists.txt`. This is the code to be added: {{{ add_library(global_planner_lib src/path_planner/global_planner/global_planner.cpp) }}} Then, in a terminal run `catkin_make` in your catkin workspace directory to generate the binary files. This will create the library file in the lib directory `~/catkin_ws/devel/lib/libglobal_planner_lib`. Observe that "lib" is appended to the library name global_planner_lib declared in the `CMakeLists.txt` == Writing your Plugin == Basically, it is important follow all the steps required to create a new plugin as explained in the [[pluginlib|plugin description page]]. There are five steps: === Plugin Registration === First, you need to register your global planner class as plugin by exporting it. In order to allow a class to be dynamically loaded, it must be marked as an exported class. This is done through the special macro PLUGINLIB_EXPORT_CLASS. This macro can be put into any source (.cpp) file that composes the [[pluginlib|plugin]] library, but is usually put at the end of the .cpp file for the exported class. This was already done above in `global_planner.cpp` with the instruction {{{ PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner) }}} This will make the class global_planner::GlobalPlanner registered as plugin for [[http://docs.ros.org/hydro/api/nav_core/html/classnav__core_1_1BaseGlobalPlanner.html|nav_core::BaseGlobalPlanner]] of the [[move_base]]. === Plugin Description File === The second step consists in describing the plugin in an description file. The plugin description file is an XML file that serves to store all the important information about a plugin in a machine readable format. It contains information about the library the plugin is in, the name of the plugin, the type of the plugin, etc. In our case of global planner, you need to create a new file and save it in certain location in your package (in our case `global_planner` package) and give it a name, for example `global_planner_plugin.xml`. The content of the plugin description file (`global_planner_plugin.xml`), would look like this: {{{ This is a global planner plugin by iroboapp project. }}} In the first line `` we specify the path to the plugin library. In this case, the path is `lib/libglobal_planner_lib`, where `lib` is the folder in the directory `~/catkin_ws/devel/` (see Compilation section above). In this line ``, we first specify the name of the global_planner plugin that we will use later in `move_base` launch file as parameter that specifies the global planner to be used in nav_core. It is typically to use the namespace (global_planner) followed by a a slash then the name of the class (`GlobalPlanner`) to specify the name of plugin. if you do not specify the name, then the name will be equal to the type, which is in this case will be `global_planner::GlobalPlanner`. It recommended to specify the name to avoid confusion. The `type` specifies the name the class that implements the plugin which is in our case `global_planner::GlobalPlanner`, and the The `base_class_type` specifies the name the base class that implements the plugin which is in our case `nav_core::BaseGlobalPlanner`. The `` tag provides a brief description about the plugin. For a detailed description of plugin description files and their associated tags/attributes please see the following [[http://wiki.ros.org/pluginlib/PluginDescriptionFile|documentation]]. '''Why Do We Need This File?''' We need this file in addition to the code macro to allow the ROS system to automatically discover, load, and reason about plugins. The plugin description file also holds important information, like a description of the plugin, that doesn't fit well in the macro. === Registering Plugin with ROS Package System === In order for [[pluginlib]] to query all available plugins on a system across all ROS packages, each package must explicitly specify the plugins it exports and which package libraries contain those plugins. A plugin provider must point to its plugin description file in its `package.xml` inside the export tag block. Note, if you have other exports they all must go in the same export field. In our global planner example, the relevant lines would look as follows: {{{ }}} The `${prefix}/` will automatically determine the full path to the file `global_planner_plugin.xml`. For a detailed discussion of exporting a plugin, please see the following [[http://wiki.ros.org/pluginlib/PluginExport|documentation]]. '''Important Note:''' In order for the above export command to work properly, the providing package must depend directly on the package containing the plugin interface, which is [[nav_core]] in the case of global planner. So, the `global_planner` package must have the line below in its `package.xml`: {{{ nav_core nav_core }}} This will tell the compiler about the dependancy on the [[nav_core]] package. === Querying ROS Package System For Available Plugins === One can query the ROS package system via rospack to see which plugins are available by any given package. For example: {{{ rospack plugins --attrib=plugin nav_core }}} This will return all plugins exported from the nav_core package. Here is an example of execution: {{{ akoubaa@anis-vbox:~/catkin_ws$ rospack plugins --attrib=plugin nav_core rotate_recovery /opt/ros/hydro/share/rotate_recovery/rotate_plugin.xml navfn /opt/ros/hydro/share/navfn/bgp_plugin.xml base_local_planner /opt/ros/hydro/share/base_local_planner/blp_plugin.xml move_slow_and_clear /opt/ros/hydro/share/move_slow_and_clear/recovery_plugin.xml global_planner /home/akoubaa/catkin_ws/src/global_planner/global_planner_plugin.xml dwa_local_planner /opt/ros/hydro/share/dwa_local_planner/blp_plugin.xml clear_costmap_recovery /opt/ros/hydro/share/clear_costmap_recovery/ccr_plugin.xml carrot_planner /opt/ros/hydro/share/carrot_planner/bgp_plugin.xml }}} Observe that our plugin is now available under the package `global_planner` and is specified in the file `/home/akoubaa/catkin_ws/src/global_planner/global_planner_plugin.xml` You can also observe the other plugins already existing in `nav_core` package, including `carrot_planner/CarrotPlanner` and `navfn`, which implements the Dijkstra algorithm. Now, your plugin in ready to use. == Running the Plugin on the Turtlebot == There are a few steps to follow to run your planner in turtlebot. First, you need to copy the package that contains your global planner (in our case `global_planner`) into the catkin workspace of your Turtlebot (e.g. catkin_ws). Then, you need to run catkin_make to export your plugin to your turtlebot ROS environment. Second, you need to make some modification to move_base configuration to specify the new planner to be used. For this, follow the three steps: 1. In ROS Hydro version, go to this folder `/opt/ros/hydro/share/turtlebot_navigation/launch/includes` {{{ $ roscd turtlebot_navigation/ $ cd launch/includes/ }}} 2. Open the file `move_base.launch.xml` (you may need `sudo` to open and be able to save) and add the new planner as parameters of the global planner, as follows: {{{ ...... .... }}} Save and close the `move_base.launch.xml`. Note that the name of the planner is `global_planner/GlobalPlanner` the same specified in `global_planner_plugin.xml`. Now, you are ready to use your new planner. 3. You must now bringup your turtlebot. You need to launch `minimal.launch`, `3dsensor.launch`, `amcl.launch.xml` and `move_base.launch.xml`. Here is an example of launch file that can be used for this purpose. {{{ }}} Note that changes made in the file `move_base.launch.xml` will now be considered when you bring-up your turtlebot with this launch file. == Testing the planner with RVIZ == After you bringup your turtlebot, you can launch the rviz using this command (in new terminal) {{{ $ roslaunch turtlebot_rviz_launchers view_navigation.launch --screen }}} {{attachment:rviz1_750w.png}} You can now add all the information you want to display in rviz by clicking on button "Add" (at the bottom). You will see the following window: {{attachment:showGlobalPath.png}} For example if you want to display the global path, click on the tab "By topic", under "move_base" category choose "/global_plan" then "Path", add display name if you want then click OK. You can add the local path in the same way. Now click on “2D nav goal” button (at the top) and choose a goal location. You can now see your robot moving to its goal. {{attachment:rviz2_750w.png}} {{attachment:terminal.png}} [[attachment:willow_garage_map.pgm]] [[attachment:willow_garage_map.yaml]]