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

Let YourPlanner derive from mbf_costmap_core::CostmapController. Include mbf_costmap_core/costmap_controller.h. Add the missing method declarations and implement them. All five pure virtual methods have to implemented. Se the interface declaration:

  58       /**
  59        * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands
  60        * to send to the base.
  61        * @param pose the current pose of the robot.
  62        * @param velocity the current velocity of the robot.
  63        * @param cmd_vel Will be filled with the velocity command to be passed to the robot base.
  64        * @param message Optional more detailed outcome as a string
  65        * @return Result code as described on ExePath action result:
  66        *         SUCCESS           = 0
  67        *         1..9 are reserved as plugin specific non-error results
  68        *         FAILURE           = 100  # Unspecified failure, only used for old, non-mfb_core based plugins
  69        *         CANCELED          = 101
  70        *         NO_VALID_CMD      = 102
  71        *         PAT_EXCEEDED      = 103
  72        *         COLLISION         = 104
  73        *         OSCILLATION       = 105
  74        *         ROBOT_STUCK       = 106
  75        *         MISSED_GOAL       = 107
  76        *         MISSED_PATH       = 108
  77        *         BLOCKED_GOAL      = 109
  78        *         BLOCKED_PATH      = 110
  79        *         INVALID_PATH      = 111
  80        *         TF_ERROR          = 112
  81        *         NOT_INITIALIZED   = 113
  82        *         INVALID_PLUGIN    = 114
  83        *         INTERNAL_ERROR    = 115
  84        *         OUT_OF_MAP        = 116  # The start and / or the goal are outside the map
  85        *         MAP_ERROR         = 117  # The map is not running properly
  86        *         STOPPED           = 118  # The controller execution has been stopped rigorously
  87        *         121..149 are reserved as plugin specific errors
  88        */

  90                                                const geometry_msgs::TwistStamped& velocity,
  91                                                geometry_msgs::TwistStamped &cmd_vel,
  92                                                std::string &message) = 0;
  93 
  94       /**
  95        * @brief Check if the goal pose has been achieved by the local planner within tolerance limits
  96        * @remark New on MBF API
  97        * @param xy_tolerance Distance tolerance in meters
  98

  99        * @return True if achieved, false otherwise
 100        */
 101       virtual bool isGoalReached(double xy_tolerance, double yaw_tolerance) = 0;
 102 
 103       /**
 104        * @brief  Set the plan that the local planner is following
 105

 106        * @return True if the plan was updated successfully, false otherwise
 107        */
 108       virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;
 109 
 110       /**
 111        * @brief Requests the planner to cancel, e.g. if it takes too much time
 112

 113        * @return True if a cancel has been successfully requested, false if not implemented.
 114        */
 115       virtual bool cancel() = 0;
 116 
 117       /**
 118        * @brief Constructs the local planner
 119        * @param name The name to give this instance of the local planner
 120