Advanced interfaces for nav_core plugins

The goal is to let the planner inherit from nav_core::BaseLocalPlanner as well as mbf_costmap_core::CostmapController. You can let the planner derive just from CostmapController, but then you will lost backward compability. So here we are going to create two separate plugins, one for every base class.

package.xml

Version 1

Add mbf_costmap_core as build dependency and run dependency. Add  <mbf_costmap_core plugin="${prefix}/mbf_plugin.xml" />  to your existing export-section.

Version 2

Add mbf_costmap_core as build and execution dependency. Add  <mbf_costmap_core plugin="${prefix}/mbf_plugin.xml" />  to your existing export-section.

mbf_plugin.xml

Add this file next to the package.xml. You can rename it as you like, but do not miss any place to adapt. Replacing the  <<< ... >>>  Parts with the same names of your nav_base plugin xml.

<library path="<<<library name>>>">
  <class name="<<<planner name>>>" type="<<<Your planner's class>>>" base_class_type="mbf_costmap_core::CostmapController">
    <description>
      <<<Fill it>>>
    </description>
  </class>
</library>

your_planner.h

Additionally to nav_core::BaseLocalPlanner let YourPlanner also derive from mbf_costmap_core::CostmapController. Include mbf_costmap_core/costmap_controller.h. Add the missing method declarations and implement them:

   1 /**
   2  * @brief Calculates the velocity command based on the current robot pose given by pose. The velocity
   3  * and message are not set. See the interface in move base flex.
   4  * @param pose Current robot pose
   5  * @param velocity
   6  * @param cmd_vel Output the velocity command
   7  * @param message
   8  * @return a status code defined in the move base flex ExePath action.
   9  */
  10 virtual uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose,
  11                                          const geometry_msgs::TwistStamped& velocity,
  12                                          geometry_msgs::TwistStamped &cmd_vel,
  13                                          std::string &message);
  14 
  15 /**
  16  * @brief Returns true, if the goal is reached. Currently does not respect the parameters given.
  17  * @param dist_tolerance Tolerance in distance to the goal
  18  * @param angle_tolerance Tolerance in the orientation to the goals orientation
  19  * @return true, if the goal is reached
  20  */
  21 virtual bool isGoalReached(double dist_tolerance, double angle_tolerance);
  22 
  23 /**
  24  * @brief Canceles the planner.
  25  * @return True on cancel success.
  26  */
  27 virtual bool cancel();

Wiki: move_base_flex/Tutorials/NavCoreBasedLocalPlannersToMBF (last edited 2018-11-08 01:22:58 by SebastianPuetz)