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_PATH    = 109
  78        *         INVALID_PATH    = 110
  79        *         TF_ERROR        = 111
  80        *         NOT_INITIALIZED = 112
  81        *         INVALID_PLUGIN  = 113
  82        *         INTERNAL_ERROR  = 114
  83        *         121..149 are reserved as plugin specific errors
  84        */
  85       virtual uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose,
  86                                                const geometry_msgs::TwistStamped& velocity,
  87                                                geometry_msgs::TwistStamped &cmd_vel,
  88                                                std::string &message) = 0;

  90       /**
  91        * @brief Check if the goal pose has been achieved by the local planner within tolerance limits
  92        * @remark New on MBF API
  93        * @param xy_tolerance Distance tolerance in meters
  94        * @param yaw_tolerance Heading tolerance in radians
  95        * @return True if achieved, false otherwise
  96        */
  97       virtual bool isGoalReached(double xy_tolerance, double yaw_tolerance) = 0;

  99       /**
 100        * @brief  Set the plan that the local planner is following
 101        * @param plan The plan to pass to the local planner
 102        * @return True if the plan was updated successfully, false otherwise
 103        */
 104       virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;

 106       /**
 107        * @brief Requests the planner to cancel, e.g. if it takes too much time
 108        * @remark New on MBF API
 109        * @return True if a cancel has been successfully requested, false if not implemented.
 110        */
 111       virtual bool cancel() = 0;

 113       /**
 114        * @brief Constructs the local planner
 115        * @param name The name to give this instance of the local planner
 116        * @param tf A pointer to a transform listener
 117        * @param costmap_ros The cost map to use for assigning costs to local plans
 118        */
 119       virtual void initialize(std::string name, ::TF *tf, costmap_2d::Costmap2DROS *costmap_ros) = 0;