git commit -m "first commit"

navigations/move_base_flex/mbf_abstract_nav/CHANGELOG.rst

@@ -0,0 +1,82 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mbf_abstract_nav
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2020-05-25)
+------------------
+* Avoid duplicated warn logging output when we cannot cancel a plugin
+* Remove unused methods and attributes from AbstractNavigationServer, which are already present at other places
+* Reuse execution slots; cleanup only at destruction
+* Enable different goal tolerance values for each action
+
+0.3.1 (2020-04-07)
+------------------
+
+0.3.0 (2020-03-31)
+------------------
+* Clean up patience exceeded method
+* Add last valid cmd time as class variable
+* Add started state and improve output messages
+* Unify license declaration to BSD-3
+* Add parameter force_stop_on_cancel to send a zero-speed command on cancelation (default: true)
+* remove explicit boost-exception dependency, Boost >= 1.69 provides exception by default.
+* Allow the user time-consuming cancel implementations
+* Rename abstract_action.h as abstract_action_base.hpp
+* Remane robot_Information.cpp as robot_information.cpp
+* Unify headers definitions and namespace intentation
+* Add parameter to actively stop once the goal is reached
+* Exit immediately from action done callbacks when action_state is CANCELED
+
+0.2.5 (2019-10-11)
+------------------
+* Update goal pose on replanning, so the feedback remains consistent
+* Fix: Reset oscillation timer after executing a recovery behavior
+* Remove debug log messages
+* Do not pass boost functions to abstract server to (de)activate costmaps.
+  Run instead abstract methods (possibly) overridden in the costmap server,
+  all costmap-related handling refactored to a new CostmapWrapper class
+* On controller execution, check that local costmap is current
+* On move_base action, use MoveBaseResult constant to fill outcome in case of oscilation
+
+0.2.4 (2019-06-16)
+------------------
+* Reduce log verbosity by combining lines and using more DEBUG
+* Concurrency container refactoring
+* Prevent LOST goals when replanning
+* Set as canceled, when goals are preempted by a new plan
+* move setAccepted to abstract action
+* moved listener notification down after setVelocity
+* fix: Correctly fill in the ExePathResult fields
+* Fix controller_patience when controller_max_retries is -1
+* Change current_twist for last_cmd_vel on exe_path/feedback
+* Replace recursive mutexes with normal ones when not needed
+* Give feedback with outcome and message for success and error cases from the plugin.
+
+0.2.3 (2018-11-14)
+------------------
+* Do not publish path from MBF
+* Single publisher for controller execution objects
+* Ignore max_retries if value is negative and patience if 0
+* Avoid annoying INFO log msg on recovery
+
+0.2.2 (2018-10-10)
+------------------
+* Add outcome and message to the action's feedback in ExePath and MoveBase
+
+0.2.1 (2018-10-03)
+------------------
+* Fix memory leak
+* Fix uninitialized value for cost
+* Make MBF melodic and indigo compatible
+* Fix GoalHandle references bug in callbacks
+
+0.2.0 (2018-09-11)
+------------------
+* Update copyright and 3-clause-BSD license
+* Concurrency for planners, controllers and recovery behaviors
+* New class structure, allowing multiple executoin instances
+* Fixes minor bugs
+
+0.1.0 (2018-03-22)
+------------------
+* First release of move_base_flex for kinetic and lunar

navigations/move_base_flex/mbf_abstract_nav/CMakeLists.txt

@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mbf_abstract_nav)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+  actionlib
+  actionlib_msgs
+  dynamic_reconfigure
+  geometry_msgs
+  mbf_msgs
+  mbf_abstract_core
+  mbf_utility
+  nav_msgs
+  roscpp
+  std_msgs
+  std_srvs
+  tf
+  xmlrpcpp
+  )
+
+find_package(Boost COMPONENTS thread chrono REQUIRED)
+
+# dynamic reconfigure: we provide the abstract configuration common to all MBF-based navigation
+# frameworks in a python module, so it can easily be included in particular navigation flavours
+catkin_python_setup()
+
+generate_dynamic_reconfigure_options(
+  cfg/MoveBaseFlex.cfg
+)
+
+set(MBF_ABSTRACT_SERVER_LIB mbf_abstract_server)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${MBF_ABSTRACT_SERVER_LIB}
+  CATKIN_DEPENDS
+      actionlib
+      actionlib_msgs
+      dynamic_reconfigure
+      geometry_msgs
+      mbf_msgs
+      mbf_abstract_core
+      mbf_utility
+      nav_msgs
+      roscpp
+      std_msgs
+      std_srvs
+      tf
+      xmlrpcpp
+  DEPENDS Boost
+)
+
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+add_library(${MBF_ABSTRACT_SERVER_LIB}
+  src/controller_action.cpp
+  src/planner_action.cpp
+  src/recovery_action.cpp
+  src/move_base_action.cpp
+  src/abstract_execution_base.cpp
+  src/abstract_navigation_server.cpp
+  src/abstract_planner_execution.cpp
+  src/abstract_controller_execution.cpp
+  src/abstract_recovery_execution.cpp
+)
+
+add_dependencies(${MBF_ABSTRACT_SERVER_LIB} ${PROJECT_NAME}_gencfg)
+add_dependencies(${MBF_ABSTRACT_SERVER_LIB} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${MBF_ABSTRACT_SERVER_LIB}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+install(TARGETS
+  ${MBF_ABSTRACT_SERVER_LIB}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+

navigations/move_base_flex/mbf_abstract_nav/README.md

@@ -0,0 +1,5 @@
+# Move Base Flex Abstract Navigation Server  {#mainpage}
+
+The mbf_abstract_nav package contains the abstract navigation server implementation of Move Base Flex (MBF). The abstract navigation server is not bound to any map representation. It provides the Actions for planning, controlling and recovering. At the time of start MBF loads all defined plugins. Therefor, it loads all plugins which are defined in the lists *planners*, *controllers* and *recovery_behaviors*. Each list holds a pair of a *name* and a *type*. The *type* defines which kind of plugin to load. The *name* defines under which name the plugin should be callable by the actions.
+
+![mbf_abstract_nav sketch](doc/images/mbf_abstract_nav_s.png)

navigations/move_base_flex/mbf_abstract_nav/cfg/MoveBaseFlex.cfg

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+PACKAGE = 'mbf_abstract_nav'
+
+from mbf_abstract_nav import add_mbf_abstract_nav_params
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t
+
+gen = ParameterGenerator()
+
+add_mbf_abstract_nav_params(gen)
+
+gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)
+exit(gen.generate(PACKAGE, "move_base_flex_node", "MoveBaseFlex"))

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action_base.hpp

@@ -0,0 +1,185 @@
+/*
+ *  Copyright 2018, Sebastian Pütz
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_action.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_ACTION_BASE_H_
+#define MBF_ABSTRACT_NAV__ABSTRACT_ACTION_BASE_H_
+
+#include <actionlib/server/action_server.h>
+#include <mbf_utility/robot_information.h>
+
+#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
+
+namespace mbf_abstract_nav
+{
+
+template <typename Action, typename Execution>
+class AbstractActionBase
+{
+ public:
+  typedef boost::shared_ptr<AbstractActionBase> Ptr;
+  typedef typename actionlib::ActionServer<Action>::GoalHandle GoalHandle;
+  typedef boost::function<void (GoalHandle &goal_handle, Execution &execution)> RunMethod;
+  typedef struct{
+    typename Execution::Ptr execution;
+    boost::thread* thread_ptr = NULL;
+    GoalHandle goal_handle;
+    bool in_use = false;
+  } ConcurrencySlot;
+
+
+  AbstractActionBase(
+      const std::string &name,
+      const mbf_utility::RobotInformation &robot_info,
+      const RunMethod run_method
+  ) : name_(name), robot_info_(robot_info), run_(run_method){}
+
+  virtual ~AbstractActionBase()
+  {
+    // cleanup threads used on executions
+    boost::lock_guard<boost::mutex> guard(slot_map_mtx_);
+    typename std::map<uint8_t, ConcurrencySlot>::iterator slot_it = concurrency_slots_.begin();
+    for (; slot_it != concurrency_slots_.end(); ++slot_it)
+    {
+      threads_.remove_thread(slot_it->second.thread_ptr);
+      delete slot_it->second.thread_ptr;
+    }
+  }
+
+  virtual void start(
+      GoalHandle &goal_handle,
+      typename Execution::Ptr execution_ptr
+  )
+  {
+    uint8_t slot = goal_handle.getGoal()->concurrency_slot;
+
+    if(goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING)
+    {
+      goal_handle.setCanceled();
+    }
+    else
+    {
+      boost::lock_guard<boost::mutex> guard(slot_map_mtx_);
+      typename std::map<uint8_t, ConcurrencySlot>::iterator slot_it = concurrency_slots_.find(slot);
+      if (slot_it != concurrency_slots_.end() && slot_it->second.in_use) {
+        // if there is already a plugin running on the same slot, cancel it
+        slot_it->second.execution->cancel();
+
+        if (slot_it->second.thread_ptr->joinable()) {
+          slot_it->second.thread_ptr->join();
+        }
+      }
+
+      // fill concurrency slot with the new goal handle, execution, and working thread
+      concurrency_slots_[slot].in_use = true;
+      concurrency_slots_[slot].goal_handle = goal_handle;
+      concurrency_slots_[slot].goal_handle.setAccepted();
+      concurrency_slots_[slot].execution = execution_ptr;
+      if (concurrency_slots_[slot].thread_ptr)
+      {
+        // cleanup previous execution; otherwise we will leak threads
+        threads_.remove_thread(concurrency_slots_[slot].thread_ptr);
+        delete concurrency_slots_[slot].thread_ptr;
+      }
+      concurrency_slots_[slot].thread_ptr =
+        threads_.create_thread(boost::bind(&AbstractActionBase::run, this, boost::ref(concurrency_slots_[slot])));
+    }
+  }
+
+  virtual void cancel(GoalHandle &goal_handle)
+  {
+    uint8_t slot = goal_handle.getGoal()->concurrency_slot;
+
+    boost::lock_guard<boost::mutex> guard(slot_map_mtx_);
+    typename std::map<uint8_t, ConcurrencySlot>::iterator slot_it = concurrency_slots_.find(slot);
+    if (slot_it != concurrency_slots_.end())
+    {
+      concurrency_slots_[slot].execution->cancel();
+    }
+  }
+
+  virtual void run(ConcurrencySlot &slot)
+  {
+    slot.execution->preRun();
+    run_(slot.goal_handle, *slot.execution);
+    ROS_DEBUG_STREAM_NAMED(name_, "Finished action \"" << name_ << "\" run method, waiting for execution thread to finish.");
+    slot.execution->join();
+    ROS_DEBUG_STREAM_NAMED(name_, "Execution completed with goal status "
+                           << (int)slot.goal_handle.getGoalStatus().status << ": "<< slot.goal_handle.getGoalStatus().text);
+    slot.execution->postRun();
+    slot.in_use = false;
+  }
+
+  virtual void reconfigureAll(
+      mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
+  {
+    boost::lock_guard<boost::mutex> guard(slot_map_mtx_);
+
+    typename std::map<uint8_t, ConcurrencySlot>::iterator iter;
+    for(iter = concurrency_slots_.begin(); iter != concurrency_slots_.end(); ++iter)
+    {
+      iter->second.execution->reconfigure(config);
+    }
+  }
+
+  virtual void cancelAll()
+  {
+    ROS_INFO_STREAM_NAMED(name_, "Cancel all goals for \"" << name_ << "\".");
+    boost::lock_guard<boost::mutex> guard(slot_map_mtx_);
+    typename std::map<uint8_t, ConcurrencySlot>::iterator iter;
+    for(iter = concurrency_slots_.begin(); iter != concurrency_slots_.end(); ++iter)
+    {
+      iter->second.execution->cancel();
+    }
+    threads_.join_all();
+  }
+
+protected:
+  const std::string &name_;
+  const mbf_utility::RobotInformation &robot_info_;
+
+  RunMethod run_;
+  boost::thread_group threads_;
+  std::map<uint8_t, ConcurrencySlot> concurrency_slots_;
+
+  boost::mutex slot_map_mtx_;
+
+};
+
+}
+
+#endif /* MBF_ABSTRACT_NAV__ABSTRACT_ACTION_BASE_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

@@ -0,0 +1,372 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_controller_execution.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_CONTROLLER_EXECUTION_H_
+#define MBF_ABSTRACT_NAV__ABSTRACT_CONTROLLER_EXECUTION_H_
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+
+#include <mbf_utility/navigation_utility.h>
+#include <mbf_abstract_core/abstract_controller.h>
+#include <mbf_utility/types.h>
+
+#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
+#include "mbf_abstract_nav/abstract_execution_base.h"
+
+namespace mbf_abstract_nav
+{
+
+/**
+ * @defgroup controller_execution Controller Execution Classes
+ * @brief The controller execution classes are derived from the AbstractControllerExecution and extends the
+ *        functionality. The base controller execution code is located in the AbstractControllerExecution.
+ */
+
+/**
+ * @brief The AbstractControllerExecution class loads and binds the controller plugin. It contains a thread
+ *        running the plugin in a cycle to move the robot. An internal state is saved and will be pulled by
+ *        server, to monitor controller execution. Due to a state change it wakes up all threads connected
+ *        to the condition variable.
+ *
+ * @ingroup abstract_server controller_execution
+ */
+  class AbstractControllerExecution : public AbstractExecutionBase
+  {
+  public:
+
+    static const double DEFAULT_CONTROLLER_FREQUENCY;
+
+    typedef boost::shared_ptr<AbstractControllerExecution > Ptr;
+
+    /**
+     * @brief Constructor
+     * @param condition Thread sleep condition variable, to wake up connected threads
+     * @param controller_plugin_type The plugin type associated with the plugin to load
+     * @param tf_listener_ptr Shared pointer to a common tf listener
+     */
+    AbstractControllerExecution(
+        const std::string &name,
+        const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
+        const ros::Publisher &vel_pub,
+        const ros::Publisher &goal_pub,
+        const TFPtr &tf_listener_ptr,
+        const MoveBaseFlexConfig &config);
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~AbstractControllerExecution();
+
+    /**
+     * @brief Starts the controller, a valid plan should be given in advance.
+     * @return false if the thread is already running, true if starting the controller succeeded!
+     */
+    virtual bool start();
+
+    /**
+     * @brief Sets a new plan to the controller execution
+     * @param plan A vector of stamped poses.
+     * @param tolerance_from_action flag that will be set to true when the new plan (action) has tolerance
+     * @param action_dist_tolerance distance to goal tolerance specific for this new plan (action)
+     * @param action_angle_tolerance angle to goal tolerance specific for this new plan (action)
+     */
+    void setNewPlan(
+      const std::vector<geometry_msgs::PoseStamped> &plan,
+      bool tolerance_from_action = false,
+      double action_dist_tolerance = 1.0,
+      double action_angle_tolerance = 3.1415);
+
+    /**
+     * @brief Cancel the controller execution.
+     * This calls the cancel method of the controller plugin, sets the cancel_ flag to true,
+     * and waits for the control loop to stop. Normally called upon aborting the navigation.
+     * @return true, if the control loop stops within a cycle time.
+     */
+    virtual bool cancel();
+
+    /**
+     * @brief Internal states
+     */
+    enum ControllerState
+    {
+      INITIALIZED,  ///< Controller has been initialized successfully.
+      STARTED,      ///< Controller has been started.
+      PLANNING,     ///< Executing the plugin.
+      NO_PLAN,      ///< The controller has been started without a plan.
+      MAX_RETRIES,  ///< Exceeded the maximum number of retries without a valid command.
+      PAT_EXCEEDED, ///< Exceeded the patience time without a valid command.
+      EMPTY_PLAN,   ///< Received an empty plan.
+      INVALID_PLAN, ///< Received an invalid plan that the controller plugin rejected.
+      NO_LOCAL_CMD, ///< Received no velocity command by the plugin, in the current cycle.
+      GOT_LOCAL_CMD,///< Got a valid velocity command from the plugin.
+      ARRIVED_GOAL, ///< The robot arrived the goal.
+      CANCELED,     ///< The controller has been canceled.
+      STOPPED,      ///< The controller has been stopped!
+      INTERNAL_ERROR///< An internal error occurred.
+    };
+
+    /**
+     * @brief Return the current state of the controller execution. Thread communication safe.
+     * @return current state, enum value of ControllerState
+     */
+    ControllerState getState();
+
+    /**
+     * @brief Returns the time of the last plugin call
+     * @return Time of the last plugin call
+     */
+    ros::Time getLastPluginCallTime();
+
+    /**
+     * @brief Returns the last velocity command calculated by the plugin. Set by setVelocityCmd method.
+     * Note that it doesn't need to be a valid command sent to the robot, as we report also failed calls
+     * to the plugin on controller action feedback.
+     * @return The last valid velocity command.
+     */
+    geometry_msgs::TwistStamped getVelocityCmd();
+
+    /**
+     * @brief Checks whether the patience duration time has been exceeded, ot not
+     * @return true, if the patience has been exceeded.
+     */
+    bool isPatienceExceeded();
+
+    /**
+     * @brief Sets the controller frequency
+     * @param frequency The controller frequency
+     * @return true, if the controller frequency has been changed / set succesfully, false otherwise
+     */
+    bool setControllerFrequency(double frequency);
+
+    /**
+     * @brief Is called by the server thread to reconfigure the controller execution,
+     *        if a user uses dynamic reconfigure to reconfigure the current state.
+     * @param config MoveBaseFlexConfig object
+     */
+    void reconfigure(const MoveBaseFlexConfig &config);
+
+    /**
+     * @brief Returns whether the robot should normally move or not. True if the controller seems to work properly.
+     * @return true, if the robot should normally move, false otherwise
+     */
+    bool isMoving();
+
+  protected:
+
+    /**
+     * @brief Request plugin for a new velocity command, given the current position, orientation, and velocity of the
+     * robot. We use this virtual method to give concrete implementations as move_base the chance to override it and do
+     * additional stuff, for example locking the costmap.
+     * @param pose the current pose of the robot.
+     * @param velocity the current velocity of the robot.
+     * @param cmd_vel Will be filled with the velocity command to be passed to the robot base.
+     * @param message Optional more detailed outcome as a string.
+     * @return Result code as described on ExePath action result and plugin's header.
+     */
+    virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped &pose,
+                                        const geometry_msgs::TwistStamped &velocity,
+                                        geometry_msgs::TwistStamped &vel_cmd, std::string &message);
+
+    /**
+     * @brief Sets the velocity command, to make it available for another thread
+     * @param vel_cmd_stamped current velocity command
+     */
+    void setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd_stamped);
+
+    //! the name of the loaded plugin
+    std::string plugin_name_;
+
+    //! the local planer to calculate the velocity command
+    mbf_abstract_core::AbstractController::Ptr controller_;
+
+    //! shared pointer to the shared tf listener
+    const TFPtr &tf_listener_ptr;
+
+    //! The current cycle start time of the last cycle run. Will by updated each cycle.
+    ros::Time last_call_time_;
+
+    //! The time the controller has been started.
+    ros::Time start_time_;
+
+    //! The time the controller responded with a success output (output < 10).
+    ros::Time last_valid_cmd_time_;
+
+    //! The maximum number of retries
+    int max_retries_;
+
+    //! The time / duration of patience, before changing the state.
+    ros::Duration patience_;
+
+    /**
+     * @brief The main run method, a thread will execute this method. It contains the main controller execution loop.
+     */
+    virtual void run();
+
+    /**
+     * @brief Check if its safe to drive.
+     * This method gets called at every controller cycle, stopping the robot if its not. When overridden by
+     * child class, gives a chance to the specific execution implementation to stop the robot if it detects
+     * something wrong on the underlying map.
+     * @return Always true, unless overridden by child class.
+     */
+    virtual bool safetyCheck() { return true; };
+
+  private:
+
+
+    /**
+     * @brief Publishes a velocity command with zero values to stop the robot.
+     */
+    void publishZeroVelocity();
+
+    /**
+     * @brief Checks whether the goal has been reached in the range of tolerance or not
+     * @return true if the goal has been reached, false otherwise
+     */
+    bool reachedGoalCheck();
+
+    /**
+     * @brief Computes the robot pose;
+     * @return true if the robot pose has been computed successfully, false otherwise.
+     */
+    bool computeRobotPose();
+
+    /**
+     * @brief Sets the controller state. This method makes the communication of the state thread safe.
+     * @param state The current controller state.
+     */
+    void setState(ControllerState state);
+
+    //! mutex to handle safe thread communication for the current value of the state
+    boost::mutex state_mtx_;
+
+    //! mutex to handle safe thread communication for the current plan
+    boost::mutex plan_mtx_;
+
+    //! mutex to handle safe thread communication for the current velocity command
+    boost::mutex vel_cmd_mtx_;
+
+    //! mutex to handle safe thread communication for the last plugin call time
+    boost::mutex lct_mtx_;
+
+    //! true, if a new plan is available. See hasNewPlan()!
+    bool new_plan_;
+
+    /**
+     * @brief Returns true if a new plan is available, false otherwise! A new plan is set by another thread!
+     * @return true, if a new plan has been set, false otherwise.
+     */
+    bool hasNewPlan();
+
+    /**
+     * @brief Gets the new available plan. This method is thread safe.
+     * @return The plan
+     */
+    std::vector<geometry_msgs::PoseStamped> getNewPlan();
+
+    //! the last calculated velocity command
+    geometry_msgs::TwistStamped vel_cmd_stamped_;
+
+    //! the last set plan which is currently processed by the controller
+    std::vector<geometry_msgs::PoseStamped> plan_;
+
+    //! the duration which corresponds with the controller frequency
+    boost::chrono::microseconds calling_duration_;
+
+    //! the frame of the robot, which will be used to determine its position
+    std::string robot_frame_;
+
+    //! the global frame the robot is controlling in
+    std::string global_frame_;
+
+    //! publisher for the current velocity command
+    ros::Publisher vel_pub_;
+
+    //! publisher for the current goal
+    ros::Publisher current_goal_pub_;
+
+    //! the current controller state
+    AbstractControllerExecution::ControllerState state_;
+
+    //! time before a timeout used for tf requests
+    double tf_timeout_;
+
+    //! dynamic reconfigure config mutex, thread safe param reading and writing
+    boost::mutex configuration_mutex_;
+
+    //! main controller loop variable, true if the controller is running, false otherwise
+    bool moving_;
+
+    //! whether MBF should check if the robot has reached the goal, or delegate on the controller instead
+    bool mbf_check_goal_reached_;
+
+    //! whether move base flex should force the robot to stop once the goal is reached
+    bool force_stop_at_goal_;
+
+    //! whether move base flex should force the robot to stop on canceling navigation
+    bool force_stop_on_cancel_;
+
+    //! distance tolerance to the given goal pose
+    double dist_tolerance_;
+
+    //! angle tolerance to the given goal pose
+    double angle_tolerance_;
+
+    //! current robot pose;
+    geometry_msgs::PoseStamped robot_pose_;
+
+    //! whether check for action specific tolerance
+    bool tolerance_from_action_;
+
+    //! replaces parameter dist_tolerance_ for the action
+    double action_dist_tolerance_;
+
+    //! replaces parameter angle_tolerance_ for the action
+    double action_angle_tolerance_;
+  };
+
+} /* namespace mbf_abstract_nav */
+
+#endif /* MBF_ABSTRACT_NAV__ABSTRACT_CONTROLLER_EXECUTION_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h

@@ -0,0 +1,120 @@
+/*
+ *  Copyright 2018, Sebastian Pütz
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_execution_base.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_EXECUTION_BASE_H_
+#define MBF_ABSTRACT_NAV__ABSTRACT_EXECUTION_BASE_H_
+
+#include <boost/thread.hpp>
+#include <boost/chrono/duration.hpp>
+#include <boost/chrono/thread_clock.hpp>
+
+#include <mbf_abstract_nav/MoveBaseFlexConfig.h>
+
+namespace mbf_abstract_nav
+{
+
+class AbstractExecutionBase
+{
+ public:
+
+  AbstractExecutionBase(std::string name);
+
+  virtual bool start();
+
+  virtual void stop();
+
+  /**
+   * @brief Cancel the plugin execution.
+   * @return true, if the plugin tries / tried to cancel the computation.
+   */
+  virtual bool cancel() = 0;
+
+  void join();
+
+  boost::cv_status waitForStateUpdate(boost::chrono::microseconds const &duration);
+
+  /**
+   * @brief Gets the current plugin execution outcome
+   */
+  uint32_t getOutcome();
+
+  /**
+   * @brief Gets the current plugin execution message
+   */
+  std::string getMessage();
+
+  /**
+   * @brief Returns the name of the corresponding plugin
+   */
+  std::string getName();
+
+  /**
+   * @brief Optional implementation-specific setup function, called right before execution.
+   */
+  virtual void preRun() { };
+
+  /**
+   * @brief Optional implementation-specific cleanup function, called right after execution.
+   */
+  virtual void postRun() { };
+
+protected:
+  virtual void run() = 0;
+
+  //! condition variable to wake up control thread
+  boost::condition_variable condition_;
+
+  //! the controlling thread object
+  boost::thread thread_;
+
+  //! flag for canceling controlling
+  bool cancel_;
+
+  //! the last received plugin execution outcome
+  uint32_t outcome_;
+
+  //! the last received plugin execution message
+  std::string message_;
+
+  //! the plugin name; not the plugin type!
+  std::string name_;
+};
+
+} /* namespace mbf_abstract_nav */
+
+#endif  /* MBF_ABSTRACT_NAV__ABSTRACT_EXECUTION_BASE_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h

@@ -0,0 +1,359 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_navigation_server.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_NAVIGATION_SERVER_H_
+#define MBF_ABSTRACT_NAV__ABSTRACT_NAVIGATION_SERVER_H_
+
+#include <string>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/recursive_mutex.hpp>
+
+#include <actionlib/server/action_server.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include <mbf_utility/navigation_utility.h>
+
+#include "mbf_abstract_nav/abstract_plugin_manager.h"
+#include "mbf_abstract_nav/abstract_planner_execution.h"
+#include "mbf_abstract_nav/abstract_controller_execution.h"
+#include "mbf_abstract_nav/abstract_recovery_execution.h"
+
+#include "mbf_abstract_nav/planner_action.h"
+#include "mbf_abstract_nav/controller_action.h"
+#include "mbf_abstract_nav/recovery_action.h"
+#include "mbf_abstract_nav/move_base_action.h"
+
+#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
+
+namespace mbf_abstract_nav
+{
+
+/**
+ * @defgroup abstract_server Abstract Server
+ * @brief Classes belonging to the Abstract Server level.
+ */
+
+/**
+ * @defgroup navigation_server Navigation Server Classes.
+ * @brief Classes combining the core logic and providing concrete implementations.
+ */
+
+
+//! GetPath action server
+typedef actionlib::ActionServer<mbf_msgs::GetPathAction> ActionServerGetPath;
+typedef boost::shared_ptr<ActionServerGetPath> ActionServerGetPathPtr;
+
+//! ExePath action server
+typedef actionlib::ActionServer<mbf_msgs::ExePathAction> ActionServerExePath;
+typedef boost::shared_ptr<ActionServerExePath> ActionServerExePathPtr;
+
+//! Recovery action server
+typedef actionlib::ActionServer<mbf_msgs::RecoveryAction> ActionServerRecovery;
+typedef boost::shared_ptr<ActionServerRecovery> ActionServerRecoveryPtr;
+
+//! MoveBase action server
+typedef actionlib::ActionServer<mbf_msgs::MoveBaseAction> ActionServerMoveBase;
+typedef boost::shared_ptr<ActionServerMoveBase> ActionServerMoveBasePtr;
+
+//! ExePath action topic name
+const std::string name_action_exe_path = "exe_path";
+//! GetPath action topic name
+const std::string name_action_get_path = "get_path";
+//! Recovery action topic name
+const std::string name_action_recovery = "recovery";
+//! MoveBase action topic name
+const std::string name_action_move_base = "move_base";
+
+
+typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBaseFlexConfig> > DynamicReconfigureServer;
+
+/**
+ * @brief The AbstractNavigationServer is the abstract base class for all navigation servers in move_base_flex
+ *        and bundles the @ref controller_execution "controller execution classes",the @ref planner_execution
+ *        "planner execution classes" and the @ref recovery_execution "recovery execution classes". It provides
+ *        the following action servers ActionServerGetPath -> callActionGetPath(), ActionServerExePath -> callActionExePath(),
+ *        ActionServerRecovery -> callActionRecovery() and ActionServerMoveBase -> callActionMoveBase().
+ *
+ * @ingroup abstract_server navigation_server
+ */
+  class AbstractNavigationServer
+  {
+  public:
+
+    /**
+     * @brief Constructor, reads all parameters and initializes all action servers and creates the plugin instances.
+     *        Parameters are the concrete implementations of the abstract classes.
+     * @param tf_listener_ptr shared pointer to the common TransformListener buffering transformations
+     */
+    AbstractNavigationServer(const TFPtr &tf_listener_ptr);
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~AbstractNavigationServer();
+
+    virtual void stop();
+
+    /**
+     * @brief Create a new abstract planner execution.
+     * @param plugin_name Name of the planner to use.
+     * @param plugin_ptr Shared pointer to the plugin to use.
+     * @return Shared pointer to a new @ref planner_execution "PlannerExecution".
+     */
+    virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr newPlannerExecution(
+        const std::string &plugin_name,
+        const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr);
+
+    /**
+     * @brief Create a new abstract controller execution.
+     * @param plugin_name Name of the controller to use.
+     * @param plugin_ptr Shared pointer to the plugin to use.
+     * @return Shared pointer to a new @ref controller_execution "ControllerExecution".
+     */
+    virtual mbf_abstract_nav::AbstractControllerExecution::Ptr newControllerExecution(
+        const std::string &plugin_name,
+        const mbf_abstract_core::AbstractController::Ptr &plugin_ptr);
+
+    /**
+     * @brief Create a new abstract recovery behavior execution.
+     * @param plugin_name Name of the recovery behavior to run.
+     * @param plugin_ptr Shared pointer to the plugin to use
+     * @return Shared pointer to a new @ref recovery_execution "RecoveryExecution".
+     */
+    virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr newRecoveryExecution(
+        const std::string &plugin_name,
+        const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr);
+
+    /**
+     * @brief Loads the plugin associated with the given planner_type parameter.
+     * @param planner_type The type of the planner plugin to load.
+     * @return Pointer to the loaded plugin
+     */
+    virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string &planner_type) = 0;
+
+    /**
+     * @brief Loads the plugin associated with the given controller type parameter
+     * @param controller_type The type of the controller plugin
+     * @return A shared pointer to a new loaded controller, if the controller plugin was loaded successfully,
+     *         an empty pointer otherwise.
+     */
+    virtual mbf_abstract_core::AbstractController::Ptr loadControllerPlugin(const std::string &controller_type) = 0;
+
+    /**
+     * @brief Loads a Recovery plugin associated with given recovery type parameter
+     * @param recovery_name The name of the Recovery plugin
+     * @return A shared pointer to a Recovery plugin, if the plugin was loaded successfully, an empty pointer otherwise.
+     */
+    virtual mbf_abstract_core::AbstractRecovery::Ptr loadRecoveryPlugin(const std::string &recovery_type) = 0;
+
+    /**
+     * @brief Pure virtual method, the derived class has to implement. Depending on the plugin base class,
+     *        some plugins need to be initialized!
+     * @param name The name of the planner
+     * @param planner_ptr pointer to the planner object which corresponds to the name param
+     * @return true if init succeeded, false otherwise
+     */
+    virtual bool initializePlannerPlugin(
+        const std::string &name,
+        const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr
+    ) = 0;
+
+    /**
+     * @brief Pure virtual method, the derived class has to implement. Depending on the plugin base class,
+     *        some plugins need to be initialized!
+     * @param name The name of the controller
+     * @param controller_ptr pointer to the controller object which corresponds to the name param
+     * @return true if init succeeded, false otherwise
+     */
+    virtual bool initializeControllerPlugin(
+        const std::string &name,
+        const mbf_abstract_core::AbstractController::Ptr &controller_ptr
+    ) = 0;
+
+    /**
+     * @brief Pure virtual method, the derived class has to implement. Depending on the plugin base class,
+     *        some plugins need to be initialized!
+     * @param name The name of the recovery behavior
+     * @param behavior_ptr pointer to the recovery behavior object which corresponds to the name param
+     * @return true if init succeeded, false otherwise
+     */
+    virtual bool initializeRecoveryPlugin(
+        const std::string &name,
+        const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr
+    ) = 0;
+
+
+    /**
+     * @brief GetPath action execution method. This method will be called if the action server receives a goal
+     * @param goal SimpleActionServer goal containing all necessary parameters for the action execution. See the action
+     *        definitions in mbf_msgs.
+     */
+    virtual void callActionGetPath(ActionServerGetPath::GoalHandle goal_handle);
+
+    virtual void cancelActionGetPath(ActionServerGetPath::GoalHandle goal_handle);
+
+    /**
+     * @brief ExePath action execution method. This method will be called if the action server receives a goal
+     * @param goal SimpleActionServer goal containing all necessary parameters for the action execution. See the action
+     *        definitions in mbf_msgs.
+     */
+    virtual void callActionExePath(ActionServerExePath::GoalHandle goal_handle);
+
+    virtual void cancelActionExePath(ActionServerExePath::GoalHandle goal_handle);
+
+    /**
+     * @brief Recovery action execution method. This method will be called if the action server receives a goal
+     * @param goal SimpleActionServer goal containing all necessary parameters for the action execution. See the action
+     *        definitions in mbf_msgs.
+     */
+    virtual void callActionRecovery(ActionServerRecovery::GoalHandle goal_handle);
+
+    virtual void cancelActionRecovery(ActionServerRecovery::GoalHandle goal_handle);
+
+    /**
+     * @brief MoveBase action execution method. This method will be called if the action server receives a goal
+     * @param goal SimpleActionServer goal containing all necessary parameters for the action execution. See the action
+     *        definitions in mbf_msgs.
+     */
+    virtual void callActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle);
+
+    virtual void cancelActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle);
+
+    /**
+     * @brief starts all action server.
+     */
+    virtual void startActionServers();
+
+    /**
+     * @brief initializes all server components. Initializing the plugins of the @ref planner_execution "Planner", the
+     *        @ref controller_execution "Controller", and the @ref recovery_execution "Recovery Behavior".
+     */
+    virtual void initializeServerComponents();
+
+  protected:
+
+    /**
+     * @brief Transforms a plan to the global frame (global_frame_) coord system.
+     * @param plan Input plan to be transformed.
+     * @param global_plan Output plan, which is then transformed to the global frame.
+     * @return true, if the transformation succeeded, false otherwise
+     */
+    bool transformPlanToGlobalFrame(std::vector<geometry_msgs::PoseStamped> &plan,
+                                    std::vector<geometry_msgs::PoseStamped> &global_plan);
+
+    /**
+     * @brief Start a dynamic reconfigure server.
+     * This must be called only if the extending doesn't create its own.
+     */
+    virtual void startDynamicReconfigureServer();
+
+    /**
+     * @brief Reconfiguration method called by dynamic reconfigure
+     * @param config Configuration parameters. See the MoveBaseFlexConfig definition.
+     * @param level bit mask, which parameters are set.
+     */
+    virtual void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level);
+
+    //! Private node handle
+    ros::NodeHandle private_nh_;
+
+    AbstractPluginManager<mbf_abstract_core::AbstractPlanner> planner_plugin_manager_;
+    AbstractPluginManager<mbf_abstract_core::AbstractController> controller_plugin_manager_;
+    AbstractPluginManager<mbf_abstract_core::AbstractRecovery> recovery_plugin_manager_;
+
+    //! shared pointer to the Recovery action server
+    ActionServerRecoveryPtr action_server_recovery_ptr_;
+
+    //! shared pointer to the ExePath action server
+    ActionServerExePathPtr action_server_exe_path_ptr_;
+
+    //! shared pointer to the GetPath action server
+    ActionServerGetPathPtr action_server_get_path_ptr_;
+
+    //! shared pointer to the MoveBase action server
+    ActionServerMoveBasePtr action_server_move_base_ptr_;
+
+    //! dynamic reconfigure server
+    DynamicReconfigureServer dsrv_;
+
+    //! configuration mutex for derived classes and other threads.
+    boost::mutex configuration_mutex_;
+
+    //! last configuration save
+    mbf_abstract_nav::MoveBaseFlexConfig last_config_;
+
+    //! the default parameter configuration save
+    mbf_abstract_nav::MoveBaseFlexConfig default_config_;
+
+    //! true, if the dynamic reconfigure has been setup.
+    bool setup_reconfigure_;
+
+    //! the robot frame, to get the current robot pose in the global_frame_
+    std::string robot_frame_;
+
+    //! the global frame, in which the robot is moving
+    std::string global_frame_;
+
+    //! timeout after tf returns without a result
+    ros::Duration tf_timeout_;
+
+    //! shared pointer to the common TransformListener
+    const TFPtr tf_listener_ptr_;
+
+    //! cmd_vel publisher for all controller execution objects
+    ros::Publisher vel_pub_;
+
+    //! current_goal publisher for all controller execution objects
+    ros::Publisher goal_pub_;
+
+    //! current robot state
+    mbf_utility::RobotInformation robot_info_;
+
+    ControllerAction controller_action_;
+    PlannerAction planner_action_;
+    RecoveryAction recovery_action_;
+    MoveBaseAction move_base_action_;
+  };
+
+} /* namespace mbf_abstract_nav */
+
+#endif /* MBF_ABSTRACT_NAV__ABSTRACT_NAVIGATION_SERVER_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/abstract_planner_execution.h

@@ -0,0 +1,313 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_planner_execution.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_
+#define MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include <geometry_msgs/PoseStamped.h>
+
+#include <mbf_abstract_core/abstract_planner.h>
+#include <mbf_utility/types.h>
+
+#include <mbf_utility/navigation_utility.h>
+
+#include "mbf_abstract_nav/abstract_execution_base.h"
+
+namespace mbf_abstract_nav
+{
+
+/**
+ * @defgroup planner_execution Planner Execution Classes
+ * @brief The planner execution classes are derived from the AbstractPlannerExecution and extend the functionality.
+ *        The base planner execution code is located in the AbstractPlannerExecution.
+ */
+
+/**
+ * @brief The AbstractPlannerExecution class loads and binds the global planner plugin. It contains a thread running
+ *        the plugin in a cycle to plan and re-plan. An internal state is saved and will be pulled by the server, which
+ *        controls the global planner execution. Due to a state change it wakes up all threads connected to the
+ *        condition variable.
+ *
+ * @ingroup abstract_server planner_execution
+ */
+  class AbstractPlannerExecution : public AbstractExecutionBase
+  {
+  public:
+
+    //! shared pointer type to the @ref planner_execution "planner execution".
+    typedef boost::shared_ptr<AbstractPlannerExecution > Ptr;
+
+    /**
+     * @brief Constructor
+     * @param name Name of this execution
+     * @param planner_ptr Pointer to the planner
+     * @param config Initial configuration for this execution
+     */
+    AbstractPlannerExecution(const std::string &name,
+                             const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr,
+                             const MoveBaseFlexConfig &config);
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~AbstractPlannerExecution();
+
+    /**
+     * @brief Returns a new plan, if one is available.
+     */
+    std::vector<geometry_msgs::PoseStamped> getPlan() const;
+
+    /**
+     * @brief Returns the last time a valid plan was available.
+     * @return time, the last valid plan was available.
+     */
+    ros::Time getLastValidPlanTime() const;
+
+    /**
+     * @brief Checks whether the patience was exceeded.
+     * @return true, if the patience duration was exceeded.
+     */
+    bool isPatienceExceeded() const;
+
+    /**
+     * @brief Internal states
+     */
+    enum PlanningState
+    {
+      INITIALIZED,  ///< Planner initialized.
+      STARTED,      ///< Planner started.
+      PLANNING,     ///< Executing the plugin.
+      FOUND_PLAN,   ///< Found a valid plan.
+      MAX_RETRIES,  ///< Exceeded the maximum number of retries without a valid command.
+      PAT_EXCEEDED, ///< Exceeded the patience time without a valid command.
+      NO_PLAN_FOUND,///< No plan has been found (MAX_RETRIES and PAT_EXCEEDED are 0).
+      CANCELED,     ///< The planner has been canceled.
+      STOPPED,      ///< The planner has been stopped.
+      INTERNAL_ERROR///< An internal error occurred.
+    };
+
+    /**
+     * @brief Returns the current internal state
+     * @return the current internal state
+     */
+    PlanningState getState() const;
+
+    /**
+     * @brief Gets planning frequency
+     */
+    double getFrequency() const { return frequency_; };
+
+    /**
+     * @brief Gets computed costs
+     * @return The costs of the computed path
+     */
+    double getCost() const;
+
+    /**
+     * @brief Cancel the planner execution. This calls the cancel method of the planner plugin.
+     * This could be useful if the computation takes too much time, or if we are aborting the navigation.
+     * @return true, if the planner plugin tries / tried to cancel the planning step.
+     */
+    virtual bool cancel();
+
+    /**
+     * @brief Sets a new goal pose for the planner execution
+     * @param goal the new goal pose
+     * @param dist_tolerance Tolerance in meters to the goal position
+     * @param angle_tolerance Tolerance in radians to the goal orientation
+     */
+    void setNewGoal(const geometry_msgs::PoseStamped &goal,
+                    double dist_tolerance, double angle_tolerance);
+
+    /**
+     * @brief Sets a new start pose for the planner execution
+     * @param start new start pose
+     */
+    void setNewStart(const geometry_msgs::PoseStamped &start);
+
+    /**
+     * @brief Sets a new star and goal pose for the planner execution
+     * @param start new start pose
+     * @param goal new goal pose
+     * @param dist_tolerance Tolerance in meters to the goal position
+     * @param angle_tolerance Tolerance in radians to the goal orientation
+     */
+    void setNewStartAndGoal(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
+                            double dist_tolerance, double angle_tolerance);
+
+    /**
+     * @brief Starts the planner execution thread with the given parameters.
+     * @param start start pose for the planning
+     * @param goal goal pose for the planning
+     * @param dist_tolerance Tolerance in meters to the goal position
+     * @param angle_tolerance Tolerance in radians to the goal orientation
+     * @return true, if the planner thread has been started, false if the thread is already running.
+     */
+    bool start(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
+               double dist_tolerance, double angle_tolerance);
+
+    /**
+     * @brief Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconfigure
+     *        to reconfigure the current state.
+     * @param config MoveBaseFlexConfig object
+     */
+    void reconfigure(const MoveBaseFlexConfig &config);
+
+  protected:
+
+    //! the local planer to calculate the robot trajectory
+    mbf_abstract_core::AbstractPlanner::Ptr planner_;
+
+    //! the name of the loaded planner plugin
+    std::string plugin_name_;
+
+    /**
+     * @brief The main run method, a thread will execute this method. It contains the main planner execution loop.
+     */
+    virtual void run();
+
+  private:
+
+    /**
+     * @brief calls the planner plugin to make a plan from the start pose to the goal pose.
+     * The final pose of the path must be within tolerance range (position and orientation)
+     * for this method to return a success outcome.
+     * @param start The start pose for planning
+     * @param goal The goal pose for planning
+     * @param dist_tolerance Tolerance in meters to the goal position
+     * @param angle_tolerance Tolerance in radians to the goal orientation
+     * @param plan The computed plan by the plugin
+     * @param cost The computed costs for the corresponding plan
+     * @param message An optional message which should correspond with the returned outcome
+     * @return An outcome number, see also the action definition in the GetPath.action file
+     */
+    virtual uint32_t makePlan(
+        const geometry_msgs::PoseStamped &start,
+        const geometry_msgs::PoseStamped &goal,
+        double dist_tolerance, double angle_tolerance,
+        std::vector<geometry_msgs::PoseStamped> &plan,
+        double &cost,
+        std::string &message);
+
+    /**
+     * @brief Sets the internal state, thread communication safe
+     * @param state the current state
+     * @param signalling set true to trigger the condition-variable for state-update
+     */
+    void setState(PlanningState state, bool signalling);
+
+    //! mutex to handle safe thread communication for the current state
+    mutable boost::mutex state_mtx_;
+
+    //! mutex to handle safe thread communication for the plan and plan-costs
+    mutable boost::mutex plan_mtx_;
+
+    //! mutex to handle safe thread communication for the goal and start pose.
+    mutable boost::mutex goal_start_mtx_;
+
+    //! mutex to handle safe thread communication for the planning_ flag.
+    mutable boost::mutex planning_mtx_;
+
+    //! dynamic reconfigure mutex for a thread safe communication
+    mutable boost::mutex configuration_mutex_;
+
+    //! true, if a new goal pose has been set, until it is used.
+    bool has_new_goal_;
+
+    //! true, if a new start pose has been set, until it is used.
+    bool has_new_start_;
+
+    //! the last call start time, updated each cycle.
+    ros::Time last_call_start_time_;
+
+    //! the last time a valid plan has been computed.
+    ros::Time last_valid_plan_time_;
+
+    //! current global plan
+    std::vector<geometry_msgs::PoseStamped> plan_;
+
+    //! current global plan cost
+    double cost_;
+
+    //! the current start pose used for planning
+    geometry_msgs::PoseStamped start_;
+
+    //! the current goal pose used for planning
+    geometry_msgs::PoseStamped goal_;
+
+    //! optional linear goal tolerance, in meters
+    double dist_tolerance_;
+
+    //! optional angular goal tolerance, in radians
+    double angle_tolerance_;
+
+    //! planning cycle frequency (used only when running full navigation; we store here for grouping parameters nicely)
+    double frequency_;
+
+    //! planning patience duration time
+    ros::Duration patience_;
+
+    //! planning max retries
+    int max_retries_;
+
+    //! main cycle variable of the execution loop
+    bool planning_;
+
+    //! robot frame used for computing the current robot pose
+    std::string robot_frame_;
+
+    //! the global frame in which the planner needs to plan
+    std::string global_frame_;
+
+    //! shared pointer to a common TransformListener
+    const TFPtr tf_listener_ptr_;
+
+    //! current internal state
+    PlanningState state_;
+
+  };
+
+} /* namespace mbf_abstract_nav */
+
+#endif /* MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/abstract_plugin_manager.h

@@ -0,0 +1,85 @@
+/*
+ *  Copyright 2018, Sebastian Pütz
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_plugin_manager.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_H_
+#define MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_H_
+
+#include <boost/function.hpp>
+
+namespace mbf_abstract_nav
+{
+
+template <typename PluginType>
+class AbstractPluginManager
+{
+ public:
+
+  typedef boost::function<typename PluginType::Ptr(const std::string &plugin)> loadPluginFunction;
+  typedef boost::function<bool (const std::string &name, const typename PluginType::Ptr &plugin_ptr)> initPluginFunction;
+
+  AbstractPluginManager(
+      const std::string &param_name,
+      const loadPluginFunction &loadPlugin,
+      const initPluginFunction &initPlugin
+  );
+
+  bool loadPlugins();
+
+  bool hasPlugin(const std::string &name);
+
+  std::string getType(const std::string &name);
+
+  const std::vector<std::string>& getLoadedNames();
+
+  typename PluginType::Ptr getPlugin(const std::string &name);
+
+  void clearPlugins();
+
+ protected:
+  std::map<std::string, typename PluginType::Ptr> plugins_;
+  std::map<std::string, std::string> plugins_type_;
+  std::vector<std::string> names_;
+  const std::string param_name_;
+  const loadPluginFunction loadPlugin_;
+  const initPluginFunction initPlugin_;
+};
+
+} /* namespace mbf_abstract_nav */
+
+#include "impl/abstract_plugin_manager.tcc"
+#endif /* MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/abstract_recovery_execution.h

@@ -0,0 +1,176 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_recovery_execution.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_RECOVERY_EXECUTION_H_
+#define MBF_ABSTRACT_NAV__ABSTRACT_RECOVERY_EXECUTION_H_
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include <mbf_abstract_core/abstract_recovery.h>
+#include <mbf_utility/types.h>
+
+#include <mbf_utility/navigation_utility.h>
+
+#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
+#include "mbf_abstract_nav/abstract_execution_base.h"
+
+
+namespace mbf_abstract_nav
+{
+
+/**
+ * @defgroup recovery_execution Recovery Execution Classes
+ * @brief The recovery execution classes are derived from the RecoveryPlannerExecution and extends the functionality.
+ *        The base recovery execution code is located in the AbstractRecoveryExecution.
+ */
+
+/**
+ * @brief The AbstractRecoveryExecution class loads and binds the recovery behavior plugin. It contains a thread
+ *        running the plugin, executing the recovery behavior. An internal state is saved and will be pulled by the
+ *        server, which controls the recovery behavior execution. Due to a state change it wakes up all threads
+ *        connected to the condition variable.
+ *
+ * @ingroup abstract_server recovery_execution
+ */
+  class AbstractRecoveryExecution : public AbstractExecutionBase
+  {
+  public:
+
+    typedef boost::shared_ptr<AbstractRecoveryExecution > Ptr;
+
+    /**
+     * @brief Constructor
+     * @param condition Thread sleep condition variable, to wake up connected threads
+     * @param tf_listener_ptr Shared pointer to a common tf listener
+     */
+    AbstractRecoveryExecution(const std::string &name,
+                              const mbf_abstract_core::AbstractRecovery::Ptr &recovery_ptr,
+                              const TFPtr &tf_listener_ptr,
+                              const MoveBaseFlexConfig &config);
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~AbstractRecoveryExecution();
+
+    /**
+     * @brief Checks whether the patience was exceeded.
+     * @return true, if the patience duration was exceeded.
+     */
+    bool isPatienceExceeded();
+
+    /**
+     * @brief Cancel the planner execution. This calls the cancel method of the planner plugin.
+     * This could be useful if the computation takes too much time, or if we are aborting the navigation.
+     * @return true, if the planner plugin tries / tried to cancel the planning step.
+     */
+    virtual bool cancel();
+
+    /**
+     * @brief internal state.
+     */
+    enum RecoveryState
+    {
+      INITIALIZED,   ///< The recovery execution has been initialized.
+      STARTED,       ///< The recovery execution thread has been started.
+      RECOVERING,    ///< The recovery behavior plugin is running.
+      WRONG_NAME,    ///< The given name could not be associated with a load behavior.
+      RECOVERY_DONE, ///< The recovery behavior execution is done.
+      CANCELED,      ///< The recovery execution was canceled.
+      STOPPED,       ///< The recovery execution has been stopped.
+      INTERNAL_ERROR ///< An internal error occurred.
+    };
+
+    /**
+     * @brief Returns the current state, thread-safe communication
+     * @return current internal state
+     */
+    AbstractRecoveryExecution::RecoveryState getState();
+
+    /**
+     * @brief Reconfigures the current configuration and reloads all parameters. This method is called from a dynamic
+     *        reconfigure tool.
+     * @param config Current MoveBaseFlexConfig object. See the MoveBaseFlex.cfg definition.
+     */
+    void reconfigure(const MoveBaseFlexConfig &config);
+
+  protected:
+
+    /**
+     * @brief Main execution method which will be executed by the recovery execution thread_.
+     */
+    virtual void run();
+
+    //! the current loaded recovery behavior
+    mbf_abstract_core::AbstractRecovery::Ptr behavior_;
+
+    //! shared pointer to common TransformListener
+    const TFPtr tf_listener_ptr_;
+
+  private:
+
+    /**
+     * @brief Sets the current internal state. This method is thread communication safe
+     * @param state The state to set.
+     */
+    void setState(RecoveryState state);
+
+    //! mutex to handle safe thread communication for the current state
+    boost::mutex state_mtx_;
+
+    //! dynamic reconfigure and start time mutexes to mutually exclude read/write configuration
+    boost::mutex conf_mtx_;
+    boost::mutex time_mtx_;
+
+    //! recovery behavior allowed time
+    ros::Duration patience_;
+
+    //! recovery behavior start time
+    ros::Time start_time_;
+
+    //! current internal state
+    RecoveryState state_;
+  };
+
+} /* namespace mbf_abstract_nav */
+
+#endif /* MBF_ABSTRACT_NAV__ABSTRACT_RECOVERY_EXECUTION_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h

@@ -0,0 +1,103 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  controller_action.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_
+#define MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_
+
+#include <actionlib/server/action_server.h>
+
+#include <mbf_msgs/ExePathAction.h>
+#include <mbf_utility/robot_information.h>
+
+#include "mbf_abstract_nav/abstract_action_base.hpp"
+#include "mbf_abstract_nav/abstract_controller_execution.h"
+
+namespace mbf_abstract_nav
+{
+
+class ControllerAction :
+    public AbstractActionBase<mbf_msgs::ExePathAction, AbstractControllerExecution>
+{
+ public:
+
+  typedef boost::shared_ptr<ControllerAction> Ptr;
+
+  ControllerAction(const std::string &name,
+                   const mbf_utility::RobotInformation &robot_info);
+
+  /**
+   * @brief Start controller action.
+   * Override abstract action version to allow updating current plan without stopping execution.
+   * @param goal_handle Reference to the goal handle received on action execution callback.
+   * @param execution_ptr Pointer to the execution descriptor.
+   */
+  void start(
+      GoalHandle &goal_handle,
+      typename AbstractControllerExecution::Ptr execution_ptr
+  );
+
+  void run(GoalHandle &goal_handle, AbstractControllerExecution &execution);
+
+protected:
+  void publishExePathFeedback(
+          GoalHandle &goal_handle,
+          uint32_t outcome, const std::string &message,
+          const geometry_msgs::TwistStamped &current_twist);
+
+  /**
+   * @brief Utility method to fill the ExePath action result in a single line
+   * @param outcome ExePath action outcome
+   * @param message ExePath action message
+   * @param result The action result to fill
+   */
+  void fillExePathResult(
+        uint32_t outcome, const std::string &message,
+        mbf_msgs::ExePathResult &result);
+
+  boost::mutex goal_mtx_; ///< lock goal handle for updating it while running
+  geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose
+  geometry_msgs::PoseStamped goal_pose_;  ///< Current goal pose
+
+};
+}
+
+
+
+#endif //MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/impl/abstract_plugin_manager.tcc

@@ -0,0 +1,162 @@
+/*
+ *  Copyright 2018, Sebastian Pütz
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_plugin_manager.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_TCC_
+#define MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_TCC_
+
+#include "mbf_abstract_nav/abstract_plugin_manager.h"
+#include <XmlRpcException.h>
+
+namespace mbf_abstract_nav{
+
+template <typename PluginType>
+AbstractPluginManager<PluginType>::AbstractPluginManager(
+    const std::string &param_name,
+    const loadPluginFunction &loadPlugin,
+    const initPluginFunction &initPlugin
+)
+  : param_name_(param_name), loadPlugin_(loadPlugin), initPlugin_(initPlugin)
+{
+}
+
+template <typename PluginType>
+bool AbstractPluginManager<PluginType>::loadPlugins()
+{
+  ros::NodeHandle private_nh("~");
+
+  XmlRpc::XmlRpcValue plugin_param_list;
+  if(!private_nh.getParam(param_name_, plugin_param_list))
+  {
+    ROS_WARN_STREAM("No " << param_name_ << " plugins configured! - Use the param \"" << param_name_ << "\", "
+        "which must be a list of tuples with a name and a type.");
+    return false;
+  }
+
+  try
+  {
+    for (int i = 0; i < plugin_param_list.size(); i++)
+    {
+      XmlRpc::XmlRpcValue elem = plugin_param_list[i];
+
+      std::string name = elem["name"];
+      std::string type = elem["type"];
+
+      if (plugins_.find(name) != plugins_.end())
+      {
+        ROS_ERROR_STREAM("The plugin \"" << name << "\" has already been loaded! Names must be unique!");
+        return false;
+      }
+      typename PluginType::Ptr plugin_ptr = loadPlugin_(type);
+      if(plugin_ptr && initPlugin_(name, plugin_ptr))
+      {
+
+        plugins_.insert(
+            std::pair<std::string, typename PluginType::Ptr>(name, plugin_ptr));
+
+        plugins_type_.insert(std::pair<std::string, std::string>(name, type)); // save name to type mapping
+        names_.push_back(name);
+
+        ROS_INFO_STREAM("The plugin with the type \"" << type << "\" has been loaded successfully under the name \""
+                                                       << name << "\".");
+      }
+      else
+      {
+        ROS_ERROR_STREAM("Could not load the plugin with the name \""
+                             << name << "\" and the type \"" << type << "\"!");
+      }
+    }
+  }
+  catch (XmlRpc::XmlRpcException &e)
+  {
+    ROS_ERROR_STREAM("Invalid parameter structure. The \""<< param_name_ << "\" parameter has to be a list of structs "
+                         << "with fields \"name\" and \"type\" of !");
+    ROS_ERROR_STREAM(e.getMessage());
+    return false;
+  }
+  // is there any plugin in the map?
+  return plugins_.empty() ? false : true;
+}
+
+template <typename PluginType>
+const std::vector<std::string>& AbstractPluginManager<PluginType>::getLoadedNames()
+{
+  return names_;
+}
+
+template <typename PluginType>
+bool AbstractPluginManager<PluginType>::hasPlugin(const std::string &name)
+{
+  return static_cast<bool>(plugins_.count(name)); // returns 1 or 0;
+}
+
+template <typename PluginType>
+std::string AbstractPluginManager<PluginType>::getType(const std::string &name)
+{
+  std::map<std::string, std::string>::iterator iter = plugins_type_.find(name);
+  return iter->second;
+}
+
+
+template <typename PluginType>
+typename PluginType::Ptr AbstractPluginManager<PluginType>::getPlugin(const std::string &name)
+{
+  typename std::map<std::string, typename PluginType::Ptr>::iterator new_plugin
+      = plugins_.find(name);
+  if(new_plugin != plugins_.end())
+  {
+    ROS_DEBUG_STREAM("Found plugin with the name \"" << name << "\".");
+    return new_plugin->second;
+  }
+  else
+  {
+    ROS_WARN_STREAM("The plugin with the name \"" << name << "\" has not yet been loaded!");
+    return typename PluginType::Ptr(); // return null ptr
+  }
+}
+
+template <typename PluginType>
+void AbstractPluginManager<PluginType>::clearPlugins() {
+  plugins_.clear();
+  plugins_type_.clear();
+  names_.clear();
+}
+
+} /* namespace mbf_abstract_nav */
+
+#endif //MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_TCC_
+

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h

@@ -0,0 +1,192 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  move_base_action.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+#ifndef MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_
+#define MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_
+
+#include <actionlib/server/action_server.h>
+#include <actionlib/client/simple_action_client.h>
+
+#include <mbf_msgs/MoveBaseAction.h>
+#include <mbf_msgs/GetPathAction.h>
+#include <mbf_msgs/ExePathAction.h>
+#include <mbf_msgs/RecoveryAction.h>
+
+#include <mbf_utility/robot_information.h>
+
+#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
+
+
+namespace mbf_abstract_nav
+{
+
+class MoveBaseAction
+{
+ public:
+
+  //! Action clients for the MoveBase action
+  typedef actionlib::SimpleActionClient<mbf_msgs::GetPathAction> ActionClientGetPath;
+  typedef actionlib::SimpleActionClient<mbf_msgs::ExePathAction> ActionClientExePath;
+  typedef actionlib::SimpleActionClient<mbf_msgs::RecoveryAction> ActionClientRecovery;
+
+  typedef actionlib::ActionServer<mbf_msgs::MoveBaseAction>::GoalHandle GoalHandle;
+
+  MoveBaseAction(const std::string &name,
+                 const mbf_utility::RobotInformation &robot_info,
+                 const std::vector<std::string> &controllers);
+
+  ~MoveBaseAction();
+
+  void start(GoalHandle &goal_handle);
+
+  void cancel();
+
+  void reconfigure(
+      mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level);
+
+ protected:
+
+  void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback);
+
+  void actionGetPathDone(
+      const actionlib::SimpleClientGoalState &state,
+      const mbf_msgs::GetPathResultConstPtr &result);
+
+  void actionExePathActive();
+
+  void actionExePathDone(
+      const actionlib::SimpleClientGoalState &state,
+      const mbf_msgs::ExePathResultConstPtr &result);
+
+  void actionGetPathReplanningDone(
+      const actionlib::SimpleClientGoalState &state,
+      const mbf_msgs::GetPathResultConstPtr &result);
+
+  void actionRecoveryDone(
+      const actionlib::SimpleClientGoalState &state,
+      const mbf_msgs::RecoveryResultConstPtr &result);
+
+  bool attemptRecovery();
+
+  /**
+   * Utility method that fills move base action result with the result of any of the action clients.
+   * @tparam ResultType
+   * @param result
+   * @param move_base_result
+   */
+  template <typename ResultType>
+  void fillMoveBaseResult(const ResultType& result, mbf_msgs::MoveBaseResult& move_base_result)
+  {
+    // copy outcome and message from action client result
+    move_base_result.outcome = result.outcome;
+    move_base_result.message = result.message;
+    move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
+    move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
+    move_base_result.final_pose = robot_pose_;
+  }
+
+  mbf_msgs::ExePathGoal exe_path_goal_;
+  mbf_msgs::GetPathGoal get_path_goal_;
+  mbf_msgs::RecoveryGoal recovery_goal_;
+
+  geometry_msgs::PoseStamped last_oscillation_pose_;
+  ros::Time last_oscillation_reset_;
+
+  //! timeout after a oscillation is detected
+  ros::Duration oscillation_timeout_;
+
+  //! minimal move distance to not detect an oscillation
+  double oscillation_distance_;
+
+  GoalHandle goal_handle_;
+
+  std::string name_;
+
+  //! current robot state
+  const mbf_utility::RobotInformation &robot_info_;
+
+  //! current robot pose; updated with exe_path action feedback
+  geometry_msgs::PoseStamped robot_pose_;
+
+  //! current goal pose; used to compute remaining distance and angle
+  geometry_msgs::PoseStamped goal_pose_;
+
+  ros::NodeHandle private_nh_;
+
+  //! Action client used by the move_base action
+  ActionClientExePath action_client_exe_path_;
+
+  //! Action client used by the move_base action
+  ActionClientGetPath action_client_get_path_;
+
+  //! Action client used by the move_base action
+  ActionClientRecovery action_client_recovery_;
+
+  bool replanning_;
+  ros::Rate replanning_rate_;
+  boost::mutex replanning_mtx_;
+
+  //! true, if recovery behavior for the MoveBase action is enabled.
+  bool recovery_enabled_;
+
+  std::vector<std::string> recovery_behaviors_;
+
+  std::vector<std::string>::iterator current_recovery_behavior_;
+
+  const std::vector<std::string> &behaviors_;
+
+  enum MoveBaseActionState
+  {
+    NONE,
+    GET_PATH,
+    EXE_PATH,
+    RECOVERY,
+    OSCILLATING,
+    SUCCEEDED,
+    CANCELED,
+    FAILED
+  };
+
+  MoveBaseActionState action_state_;
+  MoveBaseActionState recovery_trigger_;
+};
+
+} /* mbf_abstract_nav */
+
+#endif //MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/planner_action.h

@@ -0,0 +1,92 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  planner_action.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__PLANNER_ACTION_H_
+#define MBF_ABSTRACT_NAV__PLANNER_ACTION_H_
+
+#include <actionlib/server/action_server.h>
+
+#include <mbf_msgs/GetPathAction.h>
+#include <mbf_utility/robot_information.h>
+
+#include "mbf_abstract_nav/abstract_action_base.hpp"
+#include "mbf_abstract_nav/abstract_planner_execution.h"
+
+
+namespace mbf_abstract_nav{
+
+
+class PlannerAction : public AbstractActionBase<mbf_msgs::GetPathAction, AbstractPlannerExecution>
+{
+ public:
+
+  typedef boost::shared_ptr<PlannerAction> Ptr;
+
+  PlannerAction(
+      const std::string &name,
+      const mbf_utility::RobotInformation &robot_info
+  );
+
+  void run(GoalHandle &goal_handle, AbstractPlannerExecution &execution);
+
+ protected:
+
+  /**
+   * @brief Transforms a plan to the global frame (global_frame_) coord system.
+   * @param plan Input plan to be transformed.
+   * @param global_plan Output plan, which is then transformed to the global frame.
+   * @return true, if the transformation succeeded, false otherwise
+   */
+  bool transformPlanToGlobalFrame(std::vector<geometry_msgs::PoseStamped> &plan,
+                                  std::vector<geometry_msgs::PoseStamped> &global_plan);
+
+
+ private:
+
+  //! Publisher to publish the current goal pose, which is used for path planning
+  ros::Publisher current_goal_pub_;
+
+  //! Path sequence counter
+  unsigned int path_seq_count_;
+};
+
+} /* mbf_abstract_nav */
+
+#endif /* MBF_ABSTRACT_NAV__PLANNER_ACTION_H_ */

navigations/move_base_flex/mbf_abstract_nav/include/mbf_abstract_nav/recovery_action.h

@@ -0,0 +1,70 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  recovery_action.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_NAV__RECOVERY_ACTION_H_
+#define MBF_ABSTRACT_NAV__RECOVERY_ACTION_H_
+
+#include <boost/thread/condition_variable.hpp>
+#include <actionlib/server/action_server.h>
+
+#include <mbf_msgs/RecoveryAction.h>
+#include <mbf_utility/robot_information.h>
+
+#include "mbf_abstract_nav/abstract_action_base.hpp"
+#include "mbf_abstract_nav/abstract_recovery_execution.h"
+
+namespace mbf_abstract_nav
+{
+
+class RecoveryAction : public AbstractActionBase<mbf_msgs::RecoveryAction, AbstractRecoveryExecution>
+{
+ public:
+
+  typedef boost::shared_ptr<RecoveryAction> Ptr;
+
+  RecoveryAction(const std::string &name, const mbf_utility::RobotInformation &robot_info);
+
+  void run(GoalHandle &goal_handle, AbstractRecoveryExecution &execution);
+
+};
+
+} /* mbf_abstract_nav */
+
+#endif /* MBF_ABSTRACT_NAV__RECOVERY_ACTION_H_ */

navigations/move_base_flex/mbf_abstract_nav/package.xml

@@ -0,0 +1,46 @@
+<package>
+    <name>mbf_abstract_nav</name>
+    <version>0.3.2</version>
+    <description>
+      The mbf_abstract_nav package contains the abstract navigation server implementation of Move Base Flex (MBF). The abstract navigation server is not bound to any map representation. It provides the actions for planning, controlling and recovering. MBF loads all defined plugins at the program start. Therefor, it loads all plugins which are defined in the lists *planners*, *controllers* and *recovery_behaviors*. Each list holds a pair of a *name* and a *type*. The *type* defines which kind of plugin to load. The *name* defines under which name the plugin should be callable by the actions.
+    </description>
+    <url>http://wiki.ros.org/move_base_flex</url>
+    <author email="spuetz@uos.de">Sebastian Pütz</author>
+    <maintainer email="spuetz@uos.de">Sebastian Pütz</maintainer>
+    <maintainer email="santos@magazino.eu">Jorge Santos</maintainer>
+    <license>BSD-3</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>tf</build_depend>
+    <build_depend>roscpp</build_depend>
+    <build_depend>actionlib</build_depend>
+    <build_depend>actionlib_msgs</build_depend>
+    <build_depend>dynamic_reconfigure</build_depend>
+    <build_depend>std_msgs</build_depend>
+    <build_depend>std_srvs</build_depend>
+    <build_depend>nav_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <build_depend>mbf_abstract_core</build_depend>
+    <build_depend>mbf_msgs</build_depend>
+    <build_depend>mbf_utility</build_depend>
+    <build_depend>xmlrpcpp</build_depend>
+
+    <run_depend>tf</run_depend>
+    <run_depend>roscpp</run_depend>
+    <run_depend>actionlib</run_depend>
+    <run_depend>actionlib_msgs</run_depend>
+    <run_depend>dynamic_reconfigure</run_depend>
+    <run_depend>std_msgs</run_depend>
+    <run_depend>std_srvs</run_depend>
+    <run_depend>nav_msgs</run_depend>
+    <run_depend>geometry_msgs</run_depend>
+    <run_depend>mbf_abstract_core</run_depend>
+    <run_depend>mbf_msgs</run_depend>
+    <run_depend>mbf_utility</run_depend>
+    <run_depend>xmlrpcpp</run_depend>
+
+    <export>
+      <rosdoc config="rosdoc.yaml" />
+    </export>
+</package>

navigations/move_base_flex/mbf_abstract_nav/rosdoc.yaml

@@ -0,0 +1,4 @@
+ - builder: doxygen
+   name: Move Base Flex
+   homepage: http://wiki.ros.org/move_base_flex
+   file_patterns: '*.c *.cpp *.h *.cc *.hh *.py *.md'

navigations/move_base_flex/mbf_abstract_nav/setup.py

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages = ['mbf_abstract_nav'],
+    package_dir = {'': 'src'},
+)
+
+setup(**d)

navigations/move_base_flex/mbf_abstract_nav/src/abstract_controller_execution.cpp

@@ -0,0 +1,475 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_controller_execution.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include <mbf_msgs/ExePathResult.h>
+
+#include "mbf_abstract_nav/abstract_controller_execution.h"
+
+namespace mbf_abstract_nav
+{
+
+const double AbstractControllerExecution::DEFAULT_CONTROLLER_FREQUENCY = 100.0; // 100 Hz
+
+AbstractControllerExecution::AbstractControllerExecution(
+    const std::string &name,
+    const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
+    const ros::Publisher &vel_pub,
+    const ros::Publisher &goal_pub,
+    const TFPtr &tf_listener_ptr,
+    const MoveBaseFlexConfig &config) :
+  AbstractExecutionBase(name),
+    controller_(controller_ptr), tf_listener_ptr(tf_listener_ptr), state_(INITIALIZED),
+    moving_(false), max_retries_(0), patience_(0), vel_pub_(vel_pub), current_goal_pub_(goal_pub),
+    calling_duration_(boost::chrono::microseconds(static_cast<int>(1e6 / DEFAULT_CONTROLLER_FREQUENCY)))
+{
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  // non-dynamically reconfigurable parameters
+  private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
+  private_nh.param("map_frame", global_frame_, std::string("map"));
+  private_nh.param("force_stop_at_goal", force_stop_at_goal_, false);
+  private_nh.param("force_stop_on_cancel", force_stop_on_cancel_, false);
+  private_nh.param("check_goal_reached", mbf_check_goal_reached_, false);
+  private_nh.param("dist_tolerance", dist_tolerance_, 0.1);
+  private_nh.param("angle_tolerance", angle_tolerance_, M_PI / 18.0);
+  private_nh.param("tf_timeout", tf_timeout_, 1.0);
+
+  // dynamically reconfigurable parameters
+  reconfigure(config);
+}
+
+AbstractControllerExecution::~AbstractControllerExecution()
+{
+}
+
+bool AbstractControllerExecution::setControllerFrequency(double frequency)
+{
+  // set the calling duration by the moving frequency
+  if (frequency <= 0.0)
+  {
+    ROS_ERROR("Controller frequency must be greater than 0.0! No change of the frequency!");
+    return false;
+  }
+  calling_duration_ = boost::chrono::microseconds(static_cast<int>(1e6 / frequency));
+  return true;
+}
+
+void AbstractControllerExecution::reconfigure(const MoveBaseFlexConfig &config)
+{
+  boost::lock_guard<boost::mutex> guard(configuration_mutex_);
+  // Timeout granted to the controller. We keep calling it up to this time or up to max_retries times
+  // If it doesn't return within time, the navigator will cancel it and abort the corresponding action
+  patience_ = ros::Duration(config.controller_patience);
+
+  setControllerFrequency(config.controller_frequency);
+
+  max_retries_ = config.controller_max_retries;
+}
+
+
+bool AbstractControllerExecution::start()
+{
+  setState(STARTED);
+  if (moving_)
+  {
+    return false; // thread is already running.
+  }
+  moving_ = true;
+  return AbstractExecutionBase::start();
+}
+
+
+void AbstractControllerExecution::setState(ControllerState state)
+{
+  boost::lock_guard<boost::mutex> guard(state_mtx_);
+  state_ = state;
+}
+
+
+typename AbstractControllerExecution::ControllerState
+AbstractControllerExecution::getState()
+{
+  boost::lock_guard<boost::mutex> guard(state_mtx_);
+  return state_;
+}
+
+void AbstractControllerExecution::setNewPlan(
+  const std::vector<geometry_msgs::PoseStamped> &plan,
+  bool tolerance_from_action,
+  double action_dist_tolerance,
+  double action_angle_tolerance)
+{
+  if (moving_)
+  {
+    // This is fine on continuous replanning
+    ROS_DEBUG("Setting new plan while moving");
+  }
+  boost::lock_guard<boost::mutex> guard(plan_mtx_);
+  new_plan_ = true;
+
+  plan_ = plan;
+  tolerance_from_action_ = tolerance_from_action;
+  action_dist_tolerance_ = action_dist_tolerance;
+  action_angle_tolerance_ = action_angle_tolerance;
+}
+
+
+bool AbstractControllerExecution::hasNewPlan()
+{
+  boost::lock_guard<boost::mutex> guard(plan_mtx_);
+  return new_plan_;
+}
+
+
+std::vector<geometry_msgs::PoseStamped> AbstractControllerExecution::getNewPlan()
+{
+  boost::lock_guard<boost::mutex> guard(plan_mtx_);
+  new_plan_ = false;
+  return plan_;
+}
+
+
+bool AbstractControllerExecution::computeRobotPose()
+{
+  if (!mbf_utility::getRobotPose(*tf_listener_ptr, robot_frame_, global_frame_,
+                                 ros::Duration(tf_timeout_), robot_pose_))
+  {
+    ROS_ERROR_STREAM("Could not get the robot pose in the global frame. - robot frame: \""
+                         << robot_frame_ << "\"   global frame: \"" << global_frame_);
+    message_ = "Could not get the robot pose";
+    outcome_ = mbf_msgs::ExePathResult::TF_ERROR;
+    return false;
+  }
+  return true;
+}
+
+
+uint32_t AbstractControllerExecution::computeVelocityCmd(const geometry_msgs::PoseStamped &robot_pose,
+                                                         const geometry_msgs::TwistStamped &robot_velocity,
+                                                         geometry_msgs::TwistStamped &vel_cmd,
+                                                         std::string &message)
+{
+  return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
+}
+
+
+void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd)
+{
+  boost::lock_guard<boost::mutex> guard(vel_cmd_mtx_);
+  vel_cmd_stamped_ = vel_cmd;
+  if (vel_cmd_stamped_.header.stamp.isZero())
+    vel_cmd_stamped_.header.stamp = ros::Time::now();
+  // TODO what happen with frame id?
+  // TODO Add a queue here for handling the outcome, message and cmd_vel values bundled,
+  // TODO so there should be no loss of information in the feedback stream
+}
+
+
+geometry_msgs::TwistStamped AbstractControllerExecution::getVelocityCmd()
+{
+  boost::lock_guard<boost::mutex> guard(vel_cmd_mtx_);
+  return vel_cmd_stamped_;
+}
+
+
+ros::Time AbstractControllerExecution::getLastPluginCallTime()
+{
+  boost::lock_guard<boost::mutex> guard(lct_mtx_);
+  return last_call_time_;
+}
+
+
+bool AbstractControllerExecution::isPatienceExceeded()
+{
+  boost::lock_guard<boost::mutex> guard(lct_mtx_);
+  if(!patience_.isZero() && ros::Time::now() - start_time_ > patience_) // not zero -> activated, start_time handles init case
+  {
+    if(ros::Time::now() - last_call_time_ > patience_)
+    {
+      ROS_WARN_STREAM_THROTTLE(3, "The controller plugin \"" << name_ << "\" needs more time to compute in one run than the patience time!");
+      return true;
+    }
+    if(ros::Time::now() - last_valid_cmd_time_ > patience_)
+    {
+      ROS_DEBUG_STREAM("The controller plugin \"" << name_ << "\" does not return a success state (outcome < 10) for more than the patience time in multiple runs!");
+      return true;
+    }
+  }
+  return false;
+}
+
+
+bool AbstractControllerExecution::isMoving()
+{
+  return moving_;
+}
+
+bool AbstractControllerExecution::reachedGoalCheck()
+{
+  // Use action-provided tolerances if requested, or use parameter values otherwise
+  double dist_tolerance = tolerance_from_action_ ? action_dist_tolerance_ : dist_tolerance_;
+  double angle_tolerance = tolerance_from_action_ ? action_angle_tolerance_ : angle_tolerance_;
+
+  if (mbf_check_goal_reached_)
+  {
+    // MBF checks if we have reached the goal, or...
+    return mbf_utility::distance(robot_pose_, plan_.back()) < dist_tolerance &&
+           mbf_utility::angle(robot_pose_, plan_.back()) < angle_tolerance;
+  }
+  // ...we let the controller decide
+  return controller_->isGoalReached(dist_tolerance, angle_tolerance);
+}
+
+bool AbstractControllerExecution::cancel()
+{
+  // request the controller to cancel; it returns false if cancel is not implemented or rejected by the plugin
+  if (!controller_->cancel())
+  {
+    ROS_WARN_STREAM("Cancel controlling failed. Wait until the current control cycle finished!");
+  }
+  // then wait for the control cycle to stop (should happen immediately if the controller cancel returned true)
+  cancel_ = true;
+  if (waitForStateUpdate(boost::chrono::milliseconds(500)) == boost::cv_status::timeout)
+  {
+    // this situation should never happen; if it does, the action server will be unready for goals immediately sent
+    ROS_WARN_STREAM("Timeout while waiting for control cycle to stop; immediately sent goals can get stuck");
+    return false;
+  }
+  return true;
+}
+
+
+void AbstractControllerExecution::run()
+{
+  start_time_ = ros::Time::now();
+
+  // init plan
+  std::vector<geometry_msgs::PoseStamped> plan;
+  if (!hasNewPlan())
+  {
+    setState(NO_PLAN);
+    moving_ = false;
+    ROS_ERROR("robot navigation moving has no plan!");
+  }
+
+  last_valid_cmd_time_ = ros::Time();
+  int retries = 0;
+  int seq = 0;
+
+  try
+  {
+    while (moving_ && ros::ok())
+    {
+      boost::chrono::thread_clock::time_point loop_start_time = boost::chrono::thread_clock::now();
+
+      if (cancel_)
+      {
+        if (force_stop_on_cancel_)
+        {
+          publishZeroVelocity(); // command the robot to stop on canceling navigation
+        }
+        setState(CANCELED);
+        condition_.notify_all();
+        moving_ = false;
+        return;
+      }
+
+      if (!safetyCheck())
+      {
+        // the specific implementation must have detected a risk situation; at this abstract level, we
+        // cannot tell what the problem is, but anyway we command the robot to stop to avoid crashes
+        publishZeroVelocity();   // note that we still feedback command calculated by the plugin
+        boost::this_thread::sleep_for(calling_duration_);
+      }
+
+      // update plan dynamically
+      if (hasNewPlan())
+      {
+        plan = getNewPlan();
+
+        // check if plan is empty
+        if (plan.empty())
+        {
+          setState(EMPTY_PLAN);
+          condition_.notify_all();
+          moving_ = false;
+          return;
+        }
+
+        // check if plan could be set
+        if (!controller_->setPlan(plan))
+        {
+          setState(INVALID_PLAN);
+          condition_.notify_all();
+          moving_ = false;
+          return;
+        }
+        current_goal_pub_.publish(plan.back());
+      }
+
+      // compute robot pose and store it in robot_pose_
+      if (!computeRobotPose())
+      {
+        publishZeroVelocity();
+        setState(INTERNAL_ERROR);
+        condition_.notify_all();
+        moving_ = false;
+        return;
+      }
+
+      // ask planner if the goal is reached
+      if (reachedGoalCheck())
+      {
+        ROS_DEBUG_STREAM_NAMED("abstract_controller_execution", "Reached the goal!");
+        if (force_stop_at_goal_)
+        {
+          publishZeroVelocity();
+        }
+        setState(ARRIVED_GOAL);
+        // goal reached, tell it the controller
+        condition_.notify_all();
+        moving_ = false;
+        // if not, keep moving
+      }
+      else
+      {
+        setState(PLANNING);
+
+        // save time and call the plugin
+        lct_mtx_.lock();
+        last_call_time_ = ros::Time::now();
+        lct_mtx_.unlock();
+
+        // call plugin to compute the next velocity command
+        geometry_msgs::TwistStamped cmd_vel_stamped;
+        geometry_msgs::TwistStamped robot_velocity;   // TODO pass current velocity to the plugin!
+        outcome_ = computeVelocityCmd(robot_pose_, robot_velocity, cmd_vel_stamped, message_ = "");
+
+        if (outcome_ < 10)
+        {
+          setState(GOT_LOCAL_CMD);
+          vel_pub_.publish(cmd_vel_stamped.twist);
+          last_valid_cmd_time_ = ros::Time::now();
+          retries = 0;
+        }
+        else
+        {
+          boost::lock_guard<boost::mutex> guard(configuration_mutex_);
+          if (max_retries_ >= 0 && ++retries > max_retries_)
+          {
+            setState(MAX_RETRIES);
+            moving_ = false;
+          }
+          else if (isPatienceExceeded())
+          {
+            // patience limit enabled and running controller for more than patience without valid commands
+            setState(PAT_EXCEEDED);
+            moving_ = false;
+          }
+          else
+          {
+            setState(NO_LOCAL_CMD); // useful for server feedback
+          }
+          // could not compute a valid velocity command -> stop moving the robot
+          publishZeroVelocity(); // command the robot to stop; we still feedback command calculated by the plugin
+        }
+
+        // set stamped values; timestamp and frame_id should be set by the plugin; otherwise setVelocityCmd will do
+        cmd_vel_stamped.header.seq = seq++; // sequence number
+        setVelocityCmd(cmd_vel_stamped);
+        condition_.notify_all();
+      }
+
+      boost::chrono::thread_clock::time_point end_time = boost::chrono::thread_clock::now();
+      boost::chrono::microseconds execution_duration =
+          boost::chrono::duration_cast<boost::chrono::microseconds>(end_time - loop_start_time);
+      configuration_mutex_.lock();
+      boost::chrono::microseconds sleep_time = calling_duration_ - execution_duration;
+      configuration_mutex_.unlock();
+      if (moving_ && ros::ok())
+      {
+        if (sleep_time > boost::chrono::microseconds(0))
+        {
+          // interruption point
+          boost::this_thread::sleep_for(sleep_time);
+        }
+        else
+        {
+          // provide an interruption point also with 0 or negative sleep_time
+          boost::this_thread::interruption_point();
+          ROS_WARN_THROTTLE(1.0, "Calculation needs too much time to stay in the moving frequency! (%f > %f)",
+                            execution_duration.count()/1000000.0, calling_duration_.count()/1000000.0);
+        }
+      }
+    }
+  }
+  catch (const boost::thread_interrupted &ex)
+  {
+    // Controller thread interrupted; in most cases we have started a new plan
+    // Can also be that robot is oscillating or we have exceeded planner patience
+    ROS_DEBUG_STREAM("Controller thread interrupted!");
+    publishZeroVelocity();
+    setState(STOPPED);
+    condition_.notify_all();
+    moving_ = false;
+  }
+  catch (...)
+  {
+    message_ = "Unknown error occurred: " + boost::current_exception_diagnostic_information();
+    ROS_FATAL_STREAM(message_);
+    setState(INTERNAL_ERROR);
+  }
+}
+
+
+void AbstractControllerExecution::publishZeroVelocity()
+{
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = 0;
+  cmd_vel.linear.y = 0;
+  cmd_vel.linear.z = 0;
+  cmd_vel.angular.x = 0;
+  cmd_vel.angular.y = 0;
+  cmd_vel.angular.z = 0;
+  vel_pub_.publish(cmd_vel);
+}
+
+} /* namespace mbf_abstract_nav */

navigations/move_base_flex/mbf_abstract_nav/src/abstract_execution_base.cpp

@@ -0,0 +1,88 @@
+/*
+ *  Copyright 2018, Sebastian Pütz
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_execution_base.cpp
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#include "mbf_abstract_nav/abstract_execution_base.h"
+
+namespace mbf_abstract_nav
+{
+
+AbstractExecutionBase::AbstractExecutionBase(std::string name)
+  : outcome_(255), cancel_(false), name_(name)
+{
+}
+
+bool AbstractExecutionBase::start()
+{
+  thread_ = boost::thread(&AbstractExecutionBase::run, this);
+  return true;
+}
+
+void AbstractExecutionBase::stop()
+{
+  ROS_WARN_STREAM("Try to stop the plugin \"" << name_ << "\" rigorously by interrupting the thread!");
+  thread_.interrupt();
+}
+
+void AbstractExecutionBase::join(){
+  if (thread_.joinable())
+    thread_.join();
+}
+
+boost::cv_status AbstractExecutionBase::waitForStateUpdate(boost::chrono::microseconds const &duration)
+{
+  boost::mutex mutex;
+  boost::unique_lock<boost::mutex> lock(mutex);
+  return condition_.wait_for(lock, duration);
+}
+
+uint32_t AbstractExecutionBase::getOutcome()
+{
+  return outcome_;
+}
+
+std::string AbstractExecutionBase::getMessage()
+{
+  return message_;
+}
+
+std::string AbstractExecutionBase::getName()
+{
+  return name_;
+}
+
+} /* namespace mbf_abstract_nav */

navigations/move_base_flex/mbf_abstract_nav/src/abstract_navigation_server.cpp

@@ -0,0 +1,382 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_navigation_server.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include <nav_msgs/Path.h>
+
+#include "mbf_abstract_nav/abstract_navigation_server.h"
+
+namespace mbf_abstract_nav
+{
+
+AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
+    : tf_listener_ptr_(tf_listener_ptr), private_nh_("~"),
+      planner_plugin_manager_("planners",
+          boost::bind(&AbstractNavigationServer::loadPlannerPlugin, this, _1),
+          boost::bind(&AbstractNavigationServer::initializePlannerPlugin, this, _1, _2)),
+      controller_plugin_manager_("controllers",
+          boost::bind(&AbstractNavigationServer::loadControllerPlugin, this, _1),
+          boost::bind(&AbstractNavigationServer::initializeControllerPlugin, this, _1, _2)),
+      recovery_plugin_manager_("recovery_behaviors",
+          boost::bind(&AbstractNavigationServer::loadRecoveryPlugin, this, _1),
+          boost::bind(&AbstractNavigationServer::initializeRecoveryPlugin, this, _1, _2)),
+      tf_timeout_(private_nh_.param<double>("tf_timeout", 3.0)),
+      global_frame_(private_nh_.param<std::string>("global_frame", "map")),
+      robot_frame_(private_nh_.param<std::string>("robot_frame", "base_link")),
+      robot_info_(*tf_listener_ptr, global_frame_, robot_frame_, tf_timeout_),
+      controller_action_(name_action_exe_path, robot_info_),
+      planner_action_(name_action_get_path, robot_info_),
+      recovery_action_(name_action_recovery, robot_info_),
+      move_base_action_(name_action_move_base, robot_info_, recovery_plugin_manager_.getLoadedNames())
+{
+  ros::NodeHandle nh;
+
+  goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
+
+  // init cmd_vel publisher for the robot velocity
+  vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+
+  action_server_get_path_ptr_ = ActionServerGetPathPtr(
+    new ActionServerGetPath(
+      private_nh_,
+      name_action_get_path,
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionGetPath, this, _1),
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionGetPath, this, _1),
+      false));
+
+  action_server_exe_path_ptr_ = ActionServerExePathPtr(
+    new ActionServerExePath(
+      private_nh_,
+      name_action_exe_path,
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionExePath, this, _1),
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionExePath, this, _1),
+      false));
+
+  action_server_recovery_ptr_ = ActionServerRecoveryPtr(
+    new ActionServerRecovery(
+      private_nh_,
+      name_action_recovery,
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionRecovery, this, _1),
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionRecovery, this, _1),
+      false));
+
+  action_server_move_base_ptr_ = ActionServerMoveBasePtr(
+    new ActionServerMoveBase(
+      private_nh_,
+      name_action_move_base,
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionMoveBase, this, _1),
+      boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionMoveBase, this, _1),
+      false));
+
+  // XXX note that we don't start a dynamic reconfigure server, to avoid colliding with the one possibly created by
+  // the base class. If none, it should call startDynamicReconfigureServer method to start the one defined here for
+  // providing just the abstract server parameters
+}
+
+void AbstractNavigationServer::initializeServerComponents()
+{
+  planner_plugin_manager_.loadPlugins();
+  controller_plugin_manager_.loadPlugins();
+  recovery_plugin_manager_.loadPlugins();
+}
+
+AbstractNavigationServer::~AbstractNavigationServer()
+{
+
+}
+
+void AbstractNavigationServer::callActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
+{
+  const mbf_msgs::GetPathGoal &goal = *(goal_handle.getGoal().get());
+  const geometry_msgs::Point &p = goal.target_pose.pose.position;
+
+  std::string planner_name;
+  if(!planner_plugin_manager_.getLoadedNames().empty())
+  {
+    planner_name = goal.planner.empty() ? planner_plugin_manager_.getLoadedNames().front() : goal.planner;
+  }
+  else
+  {
+    mbf_msgs::GetPathResult result;
+    result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
+    result.message = "No plugins loaded at all!";
+    ROS_WARN_STREAM_NAMED("get_path", result.message);
+    goal_handle.setRejected(result, result.message);
+    return;
+  }
+
+  if(!planner_plugin_manager_.hasPlugin(planner_name))
+  {
+    mbf_msgs::GetPathResult result;
+    result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
+    result.message = "No plugin loaded with the given name \"" + goal.planner + "\"!";
+    ROS_WARN_STREAM_NAMED("get_path", result.message);
+    goal_handle.setRejected(result, result.message);
+    return;
+  }
+
+  mbf_abstract_core::AbstractPlanner::Ptr planner_plugin = planner_plugin_manager_.getPlugin(planner_name);
+  ROS_DEBUG_STREAM_NAMED("get_path", "Start action \"get_path\" using planner \"" << planner_name
+                        << "\" of type \"" << planner_plugin_manager_.getType(planner_name) << "\"");
+
+
+  if(planner_plugin)
+  {
+    mbf_abstract_nav::AbstractPlannerExecution::Ptr planner_execution
+        = newPlannerExecution(planner_name, planner_plugin);
+
+    //start another planning action
+    planner_action_.start(goal_handle, planner_execution);
+  }
+  else
+  {
+    mbf_msgs::GetPathResult result;
+    result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
+    result.message = "Internal Error: \"planner_plugin\" pointer should not be a null pointer!";
+    ROS_FATAL_STREAM_NAMED("get_path", result.message);
+    goal_handle.setRejected(result, result.message);
+  }
+}
+
+void AbstractNavigationServer::cancelActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
+{
+  ROS_INFO_STREAM_NAMED("get_path", "Cancel action \"get_path\"");
+  planner_action_.cancel(goal_handle);
+}
+
+void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle goal_handle)
+{
+  const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
+
+  std::string controller_name;
+  if(!controller_plugin_manager_.getLoadedNames().empty())
+  {
+    controller_name = goal.controller.empty() ? controller_plugin_manager_.getLoadedNames().front() : goal.controller;
+  }
+  else
+  {
+    mbf_msgs::ExePathResult result;
+    result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
+    result.message = "No plugins loaded at all!";
+    ROS_WARN_STREAM_NAMED("exe_path", result.message);
+    goal_handle.setRejected(result, result.message);
+    return;
+  }
+
+  if(!controller_plugin_manager_.hasPlugin(controller_name))
+  {
+    mbf_msgs::ExePathResult result;
+    result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
+    result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!";
+    ROS_WARN_STREAM_NAMED("exe_path", result.message);
+    goal_handle.setRejected(result, result.message);
+    return;
+  }
+
+  mbf_abstract_core::AbstractController::Ptr controller_plugin = controller_plugin_manager_.getPlugin(controller_name);
+  ROS_DEBUG_STREAM_NAMED("exe_path", "Start action \"exe_path\" using controller \"" << controller_name
+                        << "\" of type \"" << controller_plugin_manager_.getType(controller_name) << "\"");
+
+
+  if(controller_plugin)
+  {
+    mbf_abstract_nav::AbstractControllerExecution::Ptr controller_execution
+        = newControllerExecution(controller_name, controller_plugin);
+
+    // starts another controller action
+    controller_action_.start(goal_handle, controller_execution);
+  }
+  else
+  {
+    mbf_msgs::ExePathResult result;
+    result.outcome = mbf_msgs::ExePathResult::INTERNAL_ERROR;
+    result.message = "Internal Error: \"controller_plugin\" pointer should not be a null pointer!";
+    ROS_FATAL_STREAM_NAMED("exe_path", result.message);
+    goal_handle.setRejected(result, result.message);
+  }
+}
+
+void AbstractNavigationServer::cancelActionExePath(ActionServerExePath::GoalHandle goal_handle)
+{
+  ROS_INFO_STREAM_NAMED("exe_path", "Cancel action \"exe_path\"");
+  controller_action_.cancel(goal_handle);
+}
+
+void AbstractNavigationServer::callActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
+{
+  const mbf_msgs::RecoveryGoal &goal = *(goal_handle.getGoal().get());
+
+  std::string recovery_name;
+
+  if(!recovery_plugin_manager_.getLoadedNames().empty())
+  {
+    recovery_name = goal.behavior.empty() ? recovery_plugin_manager_.getLoadedNames().front() : goal.behavior;
+  }
+  else
+  {
+    mbf_msgs::RecoveryResult result;
+    result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
+    result.message = "No plugins loaded at all!";
+    ROS_WARN_STREAM_NAMED("recovery", result.message);
+    goal_handle.setRejected(result, result.message);
+    return;
+  }
+
+  if(!recovery_plugin_manager_.hasPlugin(recovery_name))
+  {
+    mbf_msgs::RecoveryResult result;
+    result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
+    result.message = "No plugin loaded with the given name \"" + goal.behavior + "\"!";
+    ROS_WARN_STREAM_NAMED("recovery", result.message);
+    goal_handle.setRejected(result, result.message);
+    return;
+  }
+
+  mbf_abstract_core::AbstractRecovery::Ptr recovery_plugin = recovery_plugin_manager_.getPlugin(recovery_name);
+  ROS_DEBUG_STREAM_NAMED("recovery", "Start action \"recovery\" using recovery \"" << recovery_name
+                        << "\" of type \"" << recovery_plugin_manager_.getType(recovery_name) << "\"");
+
+
+  if(recovery_plugin)
+  {
+    mbf_abstract_nav::AbstractRecoveryExecution::Ptr recovery_execution
+        = newRecoveryExecution(recovery_name, recovery_plugin);
+
+    recovery_action_.start(goal_handle, recovery_execution);
+  }
+  else
+  {
+    mbf_msgs::RecoveryResult result;
+    result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
+    result.message = "Internal Error: \"recovery_plugin\" pointer should not be a null pointer!";
+    ROS_FATAL_STREAM_NAMED("recovery", result.message);
+    goal_handle.setRejected(result, result.message);
+  }
+}
+
+void AbstractNavigationServer::cancelActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
+{
+  ROS_INFO_STREAM_NAMED("recovery", "Cancel action \"recovery\"");
+  recovery_action_.cancel(goal_handle);
+}
+
+void AbstractNavigationServer::callActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
+{
+  ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
+  move_base_action_.start(goal_handle);
+}
+
+void AbstractNavigationServer::cancelActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
+{
+  ROS_INFO_STREAM_NAMED("move_base", "Cancel action \"move_base\"");
+  move_base_action_.cancel();
+}
+
+mbf_abstract_nav::AbstractPlannerExecution::Ptr AbstractNavigationServer::newPlannerExecution(
+    const std::string &plugin_name,
+    const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr)
+{
+  return boost::make_shared<mbf_abstract_nav::AbstractPlannerExecution>(plugin_name, plugin_ptr, last_config_);
+}
+
+mbf_abstract_nav::AbstractControllerExecution::Ptr AbstractNavigationServer::newControllerExecution(
+    const std::string &plugin_name,
+    const mbf_abstract_core::AbstractController::Ptr &plugin_ptr)
+{
+  return boost::make_shared<mbf_abstract_nav::AbstractControllerExecution>(plugin_name, plugin_ptr, vel_pub_, goal_pub_,
+                                                                           tf_listener_ptr_, last_config_);
+}
+
+mbf_abstract_nav::AbstractRecoveryExecution::Ptr AbstractNavigationServer::newRecoveryExecution(
+    const std::string &plugin_name,
+    const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr)
+{
+  return boost::make_shared<mbf_abstract_nav::AbstractRecoveryExecution>(plugin_name, plugin_ptr,
+                                                                         tf_listener_ptr_, last_config_);
+}
+
+void AbstractNavigationServer::startActionServers()
+{
+  action_server_get_path_ptr_->start();
+  action_server_exe_path_ptr_->start();
+  action_server_recovery_ptr_->start();
+  action_server_move_base_ptr_->start();
+}
+
+void AbstractNavigationServer::startDynamicReconfigureServer()
+{
+  // dynamic reconfigure server
+  dsrv_ = boost::make_shared<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBaseFlexConfig> >(private_nh_);
+  dsrv_->setCallback(boost::bind(&AbstractNavigationServer::reconfigure, this, _1, _2));
+}
+
+void AbstractNavigationServer::reconfigure(
+  mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
+{
+  boost::lock_guard<boost::mutex> guard(configuration_mutex_);
+
+  // Make sure we have the original configuration the first time we're called, so we can restore it if needed
+  if (!setup_reconfigure_)
+  {
+    default_config_ = config;
+    setup_reconfigure_ = true;
+  }
+
+  if (config.restore_defaults)
+  {
+    config = default_config_;
+    // if someone sets restore defaults on the parameter server, prevent looping
+    config.restore_defaults = false;
+  }
+  planner_action_.reconfigureAll(config, level);
+  controller_action_.reconfigureAll(config, level);
+  recovery_action_.reconfigureAll(config, level);
+  move_base_action_.reconfigure(config, level);
+
+  last_config_ = config;
+}
+
+void AbstractNavigationServer::stop(){
+  planner_action_.cancelAll();
+  controller_action_.cancelAll();
+  recovery_action_.cancelAll();
+  move_base_action_.cancel();
+}
+
+} /* namespace mbf_abstract_nav */

navigations/move_base_flex/mbf_abstract_nav/src/abstract_planner_execution.cpp

@@ -0,0 +1,358 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_planner_execution.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "mbf_abstract_nav/abstract_planner_execution.h"
+
+namespace mbf_abstract_nav
+{
+
+AbstractPlannerExecution::AbstractPlannerExecution(
+    const std::string &name,
+    const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr,
+    const MoveBaseFlexConfig &config) :
+  AbstractExecutionBase(name),
+    planner_(planner_ptr), state_(INITIALIZED), planning_(false),
+    has_new_start_(false), has_new_goal_(false)
+{
+  ros::NodeHandle private_nh("~");
+
+  // non-dynamically reconfigurable parameters
+  private_nh.param("robot_frame", robot_frame_, std::string("base_footprint"));
+  private_nh.param("map_frame", global_frame_, std::string("map"));
+
+  // dynamically reconfigurable parameters
+  reconfigure(config);
+}
+
+AbstractPlannerExecution::~AbstractPlannerExecution()
+{
+}
+
+
+double AbstractPlannerExecution::getCost() const
+{
+  boost::lock_guard<boost::mutex> guard(plan_mtx_);
+  // copy plan and costs to output
+  // if the planner plugin do not compute costs compute costs by discrete path length
+  if(cost_ == 0 && !plan_.empty())
+  {
+    ROS_DEBUG_STREAM("Compute costs by discrete path length!");
+    double cost = 0;
+
+    geometry_msgs::PoseStamped prev_pose = plan_.front();
+    for(std::vector<geometry_msgs::PoseStamped>::const_iterator iter = plan_.begin() + 1; iter != plan_.end(); ++iter)
+    {
+      cost += mbf_utility::distance(prev_pose, *iter);
+      prev_pose = *iter;
+    }
+    return cost;
+  }
+  return cost_;
+}
+
+void AbstractPlannerExecution::reconfigure(const MoveBaseFlexConfig &config)
+{
+  boost::lock_guard<boost::mutex> guard(configuration_mutex_);
+
+  max_retries_ = config.planner_max_retries;
+  frequency_ = config.planner_frequency;
+
+  // Timeout granted to the global planner. We keep calling it up to this time or up to max_retries times
+  // If it doesn't return within time, the navigator will cancel it and abort the corresponding action
+  patience_ = ros::Duration(config.planner_patience);
+}
+
+
+typename AbstractPlannerExecution::PlanningState AbstractPlannerExecution::getState() const
+{
+  boost::lock_guard<boost::mutex> guard(state_mtx_);
+  return state_;
+}
+
+void AbstractPlannerExecution::setState(PlanningState state, bool signalling)
+{
+  boost::lock_guard<boost::mutex> guard(state_mtx_);
+  state_ = state;
+
+  // some states are quiet, most aren't
+  if(signalling)
+    condition_.notify_all();
+}
+
+
+ros::Time AbstractPlannerExecution::getLastValidPlanTime() const
+{
+  boost::lock_guard<boost::mutex> guard(plan_mtx_);
+  return last_valid_plan_time_;
+}
+
+
+bool AbstractPlannerExecution::isPatienceExceeded() const
+{
+  return !patience_.isZero() && (ros::Time::now() - last_call_start_time_ > patience_);
+}
+
+
+std::vector<geometry_msgs::PoseStamped> AbstractPlannerExecution::getPlan() const
+{
+  boost::lock_guard<boost::mutex> guard(plan_mtx_);
+  // copy plan and costs to output
+  return plan_;
+}
+
+
+void AbstractPlannerExecution::setNewGoal(const geometry_msgs::PoseStamped &goal,
+                                          double dist_tolerance, double angle_tolerance)
+{
+  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
+  goal_ = goal;
+  dist_tolerance_ = dist_tolerance;
+  angle_tolerance_ = angle_tolerance;
+  has_new_goal_ = true;
+}
+
+
+void AbstractPlannerExecution::setNewStart(const geometry_msgs::PoseStamped &start)
+{
+  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
+  start_ = start;
+  has_new_start_ = true;
+}
+
+
+void AbstractPlannerExecution::setNewStartAndGoal(const geometry_msgs::PoseStamped &start,
+                                                  const geometry_msgs::PoseStamped &goal,
+                                                  double dist_tolerance, double angle_tolerance)
+{
+  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
+  start_ = start;
+  goal_ = goal;
+  dist_tolerance_ = dist_tolerance;
+  angle_tolerance_ = angle_tolerance;
+  has_new_start_ = true;
+  has_new_goal_ = true;
+}
+
+
+bool AbstractPlannerExecution::start(const geometry_msgs::PoseStamped &start,
+                                     const geometry_msgs::PoseStamped &goal,
+                                     double dist_tolerance, double angle_tolerance)
+{
+  if (planning_)
+  {
+    return false;
+  }
+  boost::lock_guard<boost::mutex> guard(planning_mtx_);
+  planning_ = true;
+  start_ = start;
+  goal_ = goal;
+  dist_tolerance_ = dist_tolerance;
+  angle_tolerance_ = angle_tolerance;
+
+  const geometry_msgs::Point& s = start.pose.position;
+  const geometry_msgs::Point& g = goal.pose.position;
+
+  ROS_DEBUG_STREAM("Start planning from the start pose: (" << s.x << ", " << s.y << ", " << s.z << ")"
+                                 << " to the goal pose: ("<< g.x << ", " << g.y << ", " << g.z << ")");
+
+  return AbstractExecutionBase::start();
+}
+
+
+bool AbstractPlannerExecution::cancel()
+{
+  cancel_ = true; // force cancel immediately, as the call to cancel in the planner can take a while
+
+  // returns false if cancel is not implemented or rejected by the planner (will run until completion)
+  if (!planner_->cancel())
+  {
+    ROS_WARN_STREAM("Cancel planning failed or is not supported by the plugin. "
+        << "Wait until the current planning finished!");
+
+    return false;
+  }
+  return true;
+}
+
+uint32_t AbstractPlannerExecution::makePlan(const geometry_msgs::PoseStamped &start,
+                                            const geometry_msgs::PoseStamped &goal,
+                                            double dist_tolerance, double angle_tolerance,
+                                            std::vector<geometry_msgs::PoseStamped> &plan,
+                                            double &cost,
+                                            std::string &message)
+{
+  return planner_->makePlan(start, goal, dist_tolerance, angle_tolerance, plan, cost, message);
+}
+
+void AbstractPlannerExecution::run()
+{
+  setState(STARTED, false);
+  boost::lock_guard<boost::mutex> guard(planning_mtx_);
+  int retries = 0;
+  geometry_msgs::PoseStamped current_start = start_;
+  geometry_msgs::PoseStamped current_goal = goal_;
+
+  bool success = false;
+  bool make_plan = false;
+  bool exceeded = false;
+
+  last_call_start_time_ = ros::Time::now();
+  last_valid_plan_time_ = ros::Time::now();
+
+  try
+  {
+    while (planning_ && ros::ok())
+    {
+      boost::chrono::thread_clock::time_point start_time = boost::chrono::thread_clock::now();
+
+      // call the planner
+      std::vector<geometry_msgs::PoseStamped> plan;
+      double cost;
+
+      // lock goal start mutex
+      goal_start_mtx_.lock();
+      if (has_new_start_)
+      {
+        has_new_start_ = false;
+        current_start = start_;
+        ROS_INFO_STREAM("A new start pose is available. Planning with the new start pose!");
+        exceeded = false;
+        const geometry_msgs::Point& s = start_.pose.position;
+        ROS_INFO_STREAM("New planning start pose: (" << s.x << ", " << s.y << ", " << s.z << ")");
+      }
+      if (has_new_goal_)
+      {
+        has_new_goal_ = false;
+        current_goal = goal_;
+        ROS_INFO_STREAM("A new goal pose is available. Planning with the new goal pose and tolerances: "
+                        << dist_tolerance_ << " m, " << angle_tolerance_ << " rad");
+        exceeded = false;
+        const geometry_msgs::Point& g = goal_.pose.position;
+        ROS_INFO_STREAM("New goal pose: (" << g.x << ", " << g.y << ", " << g.z << ")");
+      }
+
+      make_plan = !(success || exceeded) || has_new_start_ || has_new_goal_;
+
+      // unlock goal
+      goal_start_mtx_.unlock();
+      setState(PLANNING, false);
+      if (make_plan)
+      {
+        outcome_ = makePlan(current_start, current_goal, dist_tolerance_, angle_tolerance_, plan, cost, message_);
+        success = outcome_ < 10;
+
+        boost::lock_guard<boost::mutex> guard(configuration_mutex_);
+
+        if (cancel_ && !isPatienceExceeded())
+        {
+          ROS_INFO_STREAM("The planner \"" << name_ << "\" has been canceled!"); // but not due to patience exceeded
+          planning_ = false;
+          setState(CANCELED, true);
+        }
+        else if (success)
+        {
+          ROS_DEBUG_STREAM("Successfully found a plan.");
+          exceeded = false;
+          planning_ = false;
+
+          plan_mtx_.lock();
+          plan_ = plan;
+          // todo compute the cost once!
+          cost_ = cost;
+          last_valid_plan_time_ = ros::Time::now();
+          plan_mtx_.unlock();
+          setState(FOUND_PLAN, true);
+        }
+        else if (max_retries_ >= 0 && ++retries > max_retries_)
+        {
+          ROS_INFO_STREAM("Planning reached max retries! (" << max_retries_ << ")");
+          exceeded = true;
+          planning_ = false;
+          setState(MAX_RETRIES, true);
+        }
+        else if (isPatienceExceeded())
+        {
+          // Patience exceeded is handled at two levels: here to stop retrying planning when max_retries is
+          // disabled, and on the navigation server when the planner doesn't return for more that patience seconds.
+          // In the second case, the navigation server has tried to cancel planning (possibly without success, as
+          // old nav_core-based planners do not support canceling), and we add here the fact to the log for info
+          ROS_INFO_STREAM("Planning patience (" << patience_.toSec() << "s) has been exceeded"
+                                                << (cancel_ ? "; planner canceled!" : ""));
+          exceeded = true;
+          planning_ = false;
+          setState(PAT_EXCEEDED, true);
+        }
+        else if (max_retries_ == 0 && patience_.isZero())
+        {
+          ROS_INFO_STREAM("Planning could not find a plan!");
+          exceeded = true;
+          planning_ = false;
+          setState(NO_PLAN_FOUND, true);
+        }
+        else
+        {
+          exceeded = false;
+          ROS_DEBUG_STREAM("Planning could not find a plan! Trying again...");
+        }
+      }
+      else if (cancel_)
+      {
+        ROS_INFO_STREAM("The global planner has been canceled!");
+        planning_ = false;
+        setState(CANCELED, true);
+      }
+    } // while (planning_ && ros::ok())
+  }
+  catch (const boost::thread_interrupted &ex)
+  {
+    // Planner thread interrupted; probably we have exceeded planner patience
+    ROS_WARN_STREAM("Planner thread interrupted!");
+    planning_ = false;
+    setState(STOPPED, true);
+  }
+  catch (...)
+  {
+    ROS_FATAL_STREAM("Unknown error occurred: " << boost::current_exception_diagnostic_information());
+    setState(INTERNAL_ERROR, true);
+  }
+}
+
+} /* namespace mbf_abstract_nav */
+

navigations/move_base_flex/mbf_abstract_nav/src/abstract_recovery_execution.cpp

@@ -0,0 +1,144 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  abstract_recovery_execution.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include <boost/exception/diagnostic_information.hpp>
+
+#include <mbf_abstract_nav/abstract_recovery_execution.h>
+
+namespace mbf_abstract_nav
+{
+
+AbstractRecoveryExecution::AbstractRecoveryExecution(
+    const std::string &name,
+    const mbf_abstract_core::AbstractRecovery::Ptr &recovery_ptr,
+    const TFPtr &tf_listener_ptr,
+    const MoveBaseFlexConfig &config) :
+  AbstractExecutionBase(name),
+    behavior_(recovery_ptr), tf_listener_ptr_(tf_listener_ptr), state_(INITIALIZED)
+{
+  // dynamically reconfigurable parameters
+  reconfigure(config);
+}
+
+AbstractRecoveryExecution::~AbstractRecoveryExecution()
+{
+}
+
+
+void AbstractRecoveryExecution::reconfigure(const MoveBaseFlexConfig &config)
+{
+  boost::lock_guard<boost::mutex> guard(conf_mtx_);
+
+  // Maximum time allowed to recovery behaviors. Intended as a safeward for the case a behavior hangs.
+  // If it doesn't return within time, the navigator will cancel it and abort the corresponding action.
+  patience_ = ros::Duration(config.recovery_patience);
+
+  // Nothing else to do here, as recovery_enabled is loaded and used in the navigation server
+}
+
+
+void AbstractRecoveryExecution::setState(RecoveryState state)
+{
+  boost::lock_guard<boost::mutex> guard(state_mtx_);
+  state_ = state;
+}
+
+
+typename AbstractRecoveryExecution::RecoveryState AbstractRecoveryExecution::getState()
+{
+  boost::lock_guard<boost::mutex> guard(state_mtx_);
+  return state_;
+}
+
+bool AbstractRecoveryExecution::cancel()
+{
+  cancel_ = true;
+  // returns false if cancel is not implemented or rejected by the recovery behavior (will run until completion)
+  if (!behavior_->cancel())
+  {
+    ROS_WARN_STREAM("Cancel recovery behavior \"" << name_ << "\" failed or is not supported by the plugin. "
+                        << "Wait until the current recovery behavior finished!");
+    return false;
+  }
+  return true;
+}
+
+bool AbstractRecoveryExecution::isPatienceExceeded()
+{
+  boost::lock_guard<boost::mutex> guard1(conf_mtx_);
+  boost::lock_guard<boost::mutex> guard2(time_mtx_);
+  ROS_DEBUG_STREAM("Patience: " << patience_ << ", start time: " << start_time_ << " now: " << ros::Time::now());
+  return !patience_.isZero() && (ros::Time::now() - start_time_ > patience_);
+}
+
+void AbstractRecoveryExecution::run()
+{
+  cancel_ = false; // reset the canceled state
+
+  time_mtx_.lock();
+  start_time_ = ros::Time::now();
+  time_mtx_.unlock();
+  setState(RECOVERING);
+  try
+  {
+    outcome_ = behavior_->runBehavior(message_);
+    if (cancel_)
+    {
+      setState(CANCELED);
+    }
+    else
+    {
+      setState(RECOVERY_DONE);
+    }
+  }
+  catch (boost::thread_interrupted &ex)
+  {
+    ROS_WARN_STREAM("Recovery \"" << name_ << "\" interrupted!");
+    setState(STOPPED);
+  }
+  catch (...)
+  {
+    ROS_FATAL_STREAM("Unknown error occurred in recovery behavior \"" << name_ << "\": " << boost::current_exception_diagnostic_information());
+    setState(INTERNAL_ERROR);
+  }
+  condition_.notify_one();
+}
+
+} /* namespace mbf_abstract_nav */

navigations/move_base_flex/mbf_abstract_nav/src/controller_action.cpp

@@ -0,0 +1,364 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  controller_action.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "mbf_abstract_nav/controller_action.h"
+
+namespace mbf_abstract_nav
+{
+
+ControllerAction::ControllerAction(
+    const std::string &action_name,
+    const mbf_utility::RobotInformation &robot_info)
+    : AbstractActionBase(action_name, robot_info, boost::bind(&mbf_abstract_nav::ControllerAction::run, this, _1, _2))
+{
+}
+
+void ControllerAction::start(
+    GoalHandle &goal_handle,
+    typename AbstractControllerExecution::Ptr execution_ptr
+)
+{
+  if(goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING)
+  {
+    goal_handle.setCanceled();
+    return;
+  }
+
+  uint8_t slot = goal_handle.getGoal()->concurrency_slot;
+
+  bool update_plan = false;
+  slot_map_mtx_.lock();
+  std::map<uint8_t, ConcurrencySlot>::iterator slot_it = concurrency_slots_.find(slot);
+  if(slot_it != concurrency_slots_.end() && slot_it->second.in_use)
+  {
+    boost::lock_guard<boost::mutex> goal_guard(goal_mtx_);
+    if(slot_it->second.execution->getName() == goal_handle.getGoal()->controller ||
+       goal_handle.getGoal()->controller.empty())
+    {
+      update_plan = true;
+      // Goal requests to run the same controller on the same concurrency slot already in use:
+      // we update the goal handle and pass the new plan and tolerances from the action to the
+      // execution without stopping it
+      execution_ptr = slot_it->second.execution;
+      execution_ptr->setNewPlan(goal_handle.getGoal()->path.poses,
+                                goal_handle.getGoal()->tolerance_from_action,
+                                goal_handle.getGoal()->dist_tolerance,
+                                goal_handle.getGoal()->angle_tolerance);
+      // Update also goal pose, so the feedback remains consistent
+      goal_pose_ = goal_handle.getGoal()->path.poses.back();
+      mbf_msgs::ExePathResult result;
+      fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Goal preempted by a new plan", result);
+      concurrency_slots_[slot].goal_handle.setCanceled(result, result.message);
+      concurrency_slots_[slot].goal_handle = goal_handle;
+      concurrency_slots_[slot].goal_handle.setAccepted();
+    }
+  }
+  slot_map_mtx_.unlock();
+  if(!update_plan)
+  {
+    // Otherwise run parent version of this method
+    AbstractActionBase::start(goal_handle, execution_ptr);
+  }
+}
+
+void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution &execution)
+{
+  goal_mtx_.lock();
+  // Note that we always use the goal handle stored on the concurrency slots map, as it can change when replanning
+  uint8_t slot = goal_handle.getGoal()->concurrency_slot;
+  goal_mtx_.unlock();
+
+  ROS_DEBUG_STREAM_NAMED(name_, "Start action "  << name_);
+
+  // ensure we don't provide values from previous execution on case of error before filling both poses
+  goal_pose_ = geometry_msgs::PoseStamped();
+  robot_pose_ = geometry_msgs::PoseStamped();
+
+  ros::NodeHandle private_nh("~");
+
+  double oscillation_timeout_tmp;
+  private_nh.param("oscillation_timeout", oscillation_timeout_tmp, 0.0);
+  ros::Duration oscillation_timeout(oscillation_timeout_tmp);
+
+  double oscillation_distance;
+  private_nh.param("oscillation_distance", oscillation_distance, 0.03);
+
+  mbf_msgs::ExePathResult result;
+  mbf_msgs::ExePathFeedback feedback;
+
+  typename AbstractControllerExecution::ControllerState state_moving_input;
+  bool controller_active = true;
+
+  goal_mtx_.lock();
+  const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
+
+  const std::vector<geometry_msgs::PoseStamped> &plan = goal.path.poses;
+  if (plan.empty())
+  {
+    fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan!", result);
+    goal_handle.setAborted(result, result.message);
+    ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
+    controller_active = false;
+    goal_mtx_.unlock();
+    return;
+  }
+
+  goal_pose_ = plan.back();
+  ROS_DEBUG_STREAM_NAMED(name_, "Called action \""
+      << name_ << "\" with plan:" << std::endl
+      << "frame: \"" << goal.path.header.frame_id << "\" " << std::endl
+      << "stamp: " << goal.path.header.stamp << std::endl
+      << "poses: " << goal.path.poses.size() << std::endl
+      << "goal: (" << goal_pose_.pose.position.x << ", "
+      << goal_pose_.pose.position.y << ", "
+      << goal_pose_.pose.position.z << ")");
+
+  goal_mtx_.unlock();
+
+
+  geometry_msgs::PoseStamped oscillation_pose;
+  ros::Time last_oscillation_reset = ros::Time::now();
+
+  bool first_cycle = true;
+
+  while (controller_active && ros::ok())
+  {
+    // goal_handle could change between the loop cycles due to adapting the plan
+    // with a new goal received for the same concurrency slot
+    if (!robot_info_.getRobotPose(robot_pose_))
+    {
+      controller_active = false;
+      fillExePathResult(mbf_msgs::ExePathResult::TF_ERROR, "Could not get the robot pose!", result);
+      goal_mtx_.lock();
+      goal_handle.setAborted(result, result.message);
+      goal_mtx_.unlock();
+      ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
+      break;
+    }
+
+    if (first_cycle)
+    {
+      // init oscillation pose
+      oscillation_pose = robot_pose_;
+    }
+
+    goal_mtx_.lock();
+    state_moving_input = execution.getState();
+
+    switch (state_moving_input)
+    {
+      case AbstractControllerExecution::INITIALIZED:
+        execution.setNewPlan(plan, goal.tolerance_from_action, goal.dist_tolerance, goal.angle_tolerance);
+        execution.start();
+        break;
+
+      case AbstractControllerExecution::STOPPED:
+        ROS_WARN_STREAM_NAMED(name_, "The controller has been stopped rigorously!");
+        controller_active = false;
+        result.outcome = mbf_msgs::ExePathResult::STOPPED;
+        result.message = "Controller has been stopped!";
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      case AbstractControllerExecution::CANCELED:
+        ROS_INFO_STREAM("Action \"exe_path\" canceled");
+        fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Controller canceled", result);
+        goal_handle.setCanceled(result, result.message);
+        controller_active = false;
+        break;
+
+      case AbstractControllerExecution::STARTED:
+        ROS_DEBUG_STREAM_NAMED(name_, "The moving has been started!");
+        break;
+
+      case AbstractControllerExecution::PLANNING:
+        if (execution.isPatienceExceeded())
+        {
+          ROS_INFO_STREAM("Try to cancel the plugin \"" << name_ << "\" after the patience time has been exceeded!");
+          if (execution.cancel())
+          {
+            ROS_INFO_STREAM("Successfully canceled the plugin \"" << name_ << "\" after the patience time has been exceeded!");
+          }
+        }
+        break;
+
+      case AbstractControllerExecution::MAX_RETRIES:
+        ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the maximum number of retries!");
+        controller_active = false;
+        fillExePathResult(execution.getOutcome(), execution.getMessage(), result);
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      case AbstractControllerExecution::PAT_EXCEEDED:
+        ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the patience time");
+        controller_active = false;
+        fillExePathResult(mbf_msgs::ExePathResult::PAT_EXCEEDED, execution.getMessage(), result);
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      case AbstractControllerExecution::NO_PLAN:
+        ROS_WARN_STREAM_NAMED(name_, "The controller has been started without a plan!");
+        controller_active = false;
+        fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started without a path", result);
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      case AbstractControllerExecution::EMPTY_PLAN:
+        ROS_WARN_STREAM_NAMED(name_, "The controller has received an empty plan");
+        controller_active = false;
+        fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan", result);
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      case AbstractControllerExecution::INVALID_PLAN:
+        ROS_WARN_STREAM_NAMED(name_, "The controller has received an invalid plan");
+        controller_active = false;
+        fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an invalid plan", result);
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      case AbstractControllerExecution::NO_LOCAL_CMD:
+        ROS_WARN_STREAM_THROTTLE_NAMED(3, name_, "No velocity command received from controller! "
+            << execution.getMessage());
+        publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd());
+        break;
+
+      case AbstractControllerExecution::GOT_LOCAL_CMD:
+        if (!oscillation_timeout.isZero())
+        {
+          // check if oscillating
+          if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance)
+          {
+            last_oscillation_reset = ros::Time::now();
+            oscillation_pose = robot_pose_;
+          }
+          else if (last_oscillation_reset + oscillation_timeout < ros::Time::now())
+          {
+            ROS_WARN_STREAM_NAMED(name_, "The controller is oscillating for "
+                << (ros::Time::now() - last_oscillation_reset).toSec() << "s");
+
+            execution.cancel();
+            controller_active = false;
+            fillExePathResult(mbf_msgs::ExePathResult::OSCILLATION, "Oscillation detected!", result);
+            goal_handle.setAborted(result, result.message);
+            break;
+          }
+        }
+        publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd());
+        break;
+
+      case AbstractControllerExecution::ARRIVED_GOAL:
+        ROS_DEBUG_STREAM_NAMED(name_, "Controller succeeded; arrived at goal");
+        controller_active = false;
+        fillExePathResult(mbf_msgs::ExePathResult::SUCCESS, "Controller succeeded; arrived at goal!", result);
+        goal_handle.setSucceeded(result, result.message);
+        break;
+
+      case AbstractControllerExecution::INTERNAL_ERROR:
+        ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin: " << execution.getMessage());
+        controller_active = false;
+        fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result);
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      default:
+        std::stringstream ss;
+        ss << "Internal error: Unknown state in a move base flex controller execution with the number: "
+           << static_cast<int>(state_moving_input);
+        fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, ss.str(), result);
+        ROS_FATAL_STREAM_NAMED(name_, result.message);
+        goal_handle.setAborted(result, result.message);
+        controller_active = false;
+    }
+    goal_mtx_.unlock();
+
+    if (controller_active)
+    {
+      // try to sleep a bit
+      // normally this thread should be woken up from the controller execution thread
+      // in order to transfer the results to the controller
+      execution.waitForStateUpdate(boost::chrono::milliseconds(500));
+    }
+
+    first_cycle = false;
+  }  // while (controller_active && ros::ok())
+
+  if (!controller_active)
+  {
+    ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
+  }
+  else
+  {
+    // normal on continuous replanning
+    ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
+  }
+}
+
+void ControllerAction::publishExePathFeedback(
+        GoalHandle &goal_handle,
+        uint32_t outcome, const std::string &message,
+        const geometry_msgs::TwistStamped &current_twist)
+{
+  mbf_msgs::ExePathFeedback feedback;
+  feedback.outcome = outcome;
+  feedback.message = message;
+
+  feedback.last_cmd_vel = current_twist;
+  if (feedback.last_cmd_vel.header.stamp.isZero())
+    feedback.last_cmd_vel.header.stamp = ros::Time::now();
+
+  feedback.current_pose = robot_pose_;
+  feedback.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
+  feedback.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
+  goal_handle.publishFeedback(feedback);
+}
+
+void ControllerAction::fillExePathResult(
+        uint32_t outcome, const std::string &message,
+        mbf_msgs::ExePathResult &result)
+{
+  result.outcome = outcome;
+  result.message = message;
+  result.final_pose = robot_pose_;
+  result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
+  result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
+}
+
+} /* mbf_abstract_nav */

navigations/move_base_flex/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py

@@ -0,0 +1,40 @@
+# Generic set of parameters required by any MBF-based navigation framework
+# To use:
+#
+#  from mbf_abstract_nav import add_mbf_abstract_nav_params
+#  gen = ParameterGenerator()
+#  add_mbf_abstract_nav_params(gen)
+#  ...
+# WARN: parameters added here must be copied on the specific MBF implementation reconfigure callback, e.g. in:
+#       https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp#L130
+# Also, you need to touch https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/cfg/MoveBaseFlex.cfg
+# when recompiling to ensure configuration code is regenerated with the new parameters
+
+# need this only for dataype declarations
+from dynamic_reconfigure.parameter_generator_catkin import str_t, double_t, int_t, bool_t
+
+
+def add_mbf_abstract_nav_params(gen):
+    gen.add("planner_frequency", double_t, 0,
+            "The rate in Hz at which to run the planning loop", 0, 0, 100)
+    gen.add("planner_patience", double_t, 0,
+            "How long the planner will wait in seconds in an attempt to find a valid plan before giving up", 5.0, 0, 100)
+    gen.add("planner_max_retries", int_t, 0,
+            "How many times we will recall the planner in an attempt to find a valid plan before giving up", -1, -1, 1000)
+
+    gen.add("controller_frequency", double_t, 0,
+            "The rate in Hz at which to run the control loop and send velocity commands to the base", 20, 0, 100)
+    gen.add("controller_patience", double_t, 0,
+            "How long the controller will wait in seconds without receiving a valid control before giving up", 5.0, 0, 100)
+    gen.add("controller_max_retries", int_t, 0,
+            "How many times we will recall the controller in an attempt to find a valid command before giving up", -1, -1, 1000)
+
+    gen.add("recovery_enabled", bool_t, 0,
+            "Whether or not to enable the move_base_flex recovery behaviors to attempt to clear out space", True)
+    gen.add("recovery_patience", double_t, 0,
+            "How much time we allow recovery behaviors to complete before canceling (or stopping if cancel fails) them", 15.0, 0, 100)
+
+    gen.add("oscillation_timeout", double_t, 0,
+            "How long in seconds to allow for oscillation before executing recovery behaviors", 0.0, 0, 60)
+    gen.add("oscillation_distance", double_t, 0,
+            "How far in meters the robot must move to be considered not to be oscillating", 0.5, 0, 10)

navigations/move_base_flex/mbf_abstract_nav/src/move_base_action.cpp

@@ -0,0 +1,586 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  move_base_action.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include <mbf_utility/navigation_utility.h>
+
+#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
+#include "mbf_abstract_nav/move_base_action.h"
+
+namespace mbf_abstract_nav
+{
+
+MoveBaseAction::MoveBaseAction(const std::string &name,
+                               const mbf_utility::RobotInformation &robot_info,
+                               const std::vector<std::string> &behaviors)
+  :  name_(name), robot_info_(robot_info), private_nh_("~"),
+     action_client_exe_path_(private_nh_, "exe_path"),
+     action_client_get_path_(private_nh_, "get_path"),
+     action_client_recovery_(private_nh_, "recovery"),
+     oscillation_timeout_(0),
+     oscillation_distance_(0),
+     recovery_enabled_(true),
+     behaviors_(behaviors),
+     action_state_(NONE),
+     recovery_trigger_(NONE),
+     replanning_(false),
+     replanning_rate_(1.0)
+{
+}
+
+MoveBaseAction::~MoveBaseAction()
+{
+}
+
+void MoveBaseAction::reconfigure(
+    mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
+{
+  if (config.planner_frequency > 0.0)
+  {
+    boost::lock_guard<boost::mutex> guard(replanning_mtx_);
+    if (!replanning_)
+    {
+      replanning_ = true;
+      if (action_state_ == EXE_PATH &&
+          action_client_get_path_.getState() != actionlib::SimpleClientGoalState::PENDING &&
+          action_client_get_path_.getState() != actionlib::SimpleClientGoalState::ACTIVE)
+      {
+        // exe_path is running and user has enabled replanning
+        ROS_INFO_STREAM_NAMED("move_base", "Planner frequency set to " << config.planner_frequency
+                              << ": start replanning, using the \"get_path\" action!");
+        action_client_get_path_.sendGoal(
+            get_path_goal_,
+            boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
+        );
+      }
+    }
+    replanning_rate_ = ros::Rate(config.planner_frequency);
+  }
+  else
+    replanning_ = false;
+  oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
+  oscillation_distance_ = config.oscillation_distance;
+  recovery_enabled_ = config.recovery_enabled;
+}
+
+void MoveBaseAction::cancel()
+{
+  action_state_ = CANCELED;
+
+  if (!action_client_get_path_.getState().isDone())
+  {
+    action_client_get_path_.cancelGoal();
+  }
+
+  if (!action_client_exe_path_.getState().isDone())
+  {
+    action_client_exe_path_.cancelGoal();
+  }
+
+  if (!action_client_recovery_.getState().isDone())
+  {
+    action_client_recovery_.cancelGoal();
+  }
+}
+
+void MoveBaseAction::start(GoalHandle &goal_handle)
+{
+  action_state_ = GET_PATH;
+
+  goal_handle.setAccepted();
+
+  goal_handle_ = goal_handle;
+
+  ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
+
+  const mbf_msgs::MoveBaseGoal& goal = *goal_handle.getGoal();
+
+  mbf_msgs::MoveBaseResult move_base_result;
+
+  get_path_goal_.target_pose = goal.target_pose;
+  get_path_goal_.use_start_pose = false; // use the robot pose
+  get_path_goal_.planner = goal.planner;
+  get_path_goal_.tolerance_from_action = goal.tolerance_from_action;
+  get_path_goal_.dist_tolerance = goal.dist_tolerance;
+  get_path_goal_.angle_tolerance = goal.angle_tolerance;
+
+  exe_path_goal_.controller = goal.controller;
+  exe_path_goal_.tolerance_from_action = goal.tolerance_from_action;
+  exe_path_goal_.dist_tolerance = goal.dist_tolerance;
+  exe_path_goal_.angle_tolerance = goal.angle_tolerance;
+
+  ros::Duration connection_timeout(1.0);
+
+  last_oscillation_reset_ = ros::Time::now();
+
+  // start recovering with the first behavior, use the recovery behaviors from the action request, if specified,
+  // otherwise, use all loaded behaviors.
+
+  recovery_behaviors_ = goal.recovery_behaviors.empty() ? behaviors_ : goal.recovery_behaviors;
+  current_recovery_behavior_ = recovery_behaviors_.begin();
+
+  // get the current robot pose only at the beginning, as exe_path will keep updating it as we move
+  if (!robot_info_.getRobotPose(robot_pose_))
+  {
+    ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!");
+    move_base_result.message = "Could not get the current robot pose!";
+    move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
+    goal_handle.setAborted(move_base_result, move_base_result.message);
+    return;
+  }
+  goal_pose_ = goal.target_pose;
+
+  // wait for server connections
+  if (!action_client_get_path_.waitForServer(connection_timeout) ||
+      !action_client_exe_path_.waitForServer(connection_timeout) ||
+      !action_client_recovery_.waitForServer(connection_timeout))
+  {
+    ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions: "
+        "\"get_path\", \"exe_path\", \"recovery \"!");
+    move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
+    move_base_result.message = "Could not connect to the move_base_flex actions!";
+    goal_handle.setAborted(move_base_result, move_base_result.message);
+    return;
+  }
+
+  // call get_path action server to get a first plan
+  action_client_get_path_.sendGoal(
+      get_path_goal_,
+      boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
+}
+
+void MoveBaseAction::actionExePathActive()
+{
+  ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active.");
+}
+
+void MoveBaseAction::actionExePathFeedback(
+    const mbf_msgs::ExePathFeedbackConstPtr &feedback)
+{
+  mbf_msgs::MoveBaseFeedback move_base_feedback;
+  move_base_feedback.outcome = feedback->outcome;
+  move_base_feedback.message = feedback->message;
+  move_base_feedback.angle_to_goal = feedback->angle_to_goal;
+  move_base_feedback.dist_to_goal = feedback->dist_to_goal;
+  move_base_feedback.current_pose = feedback->current_pose;
+  move_base_feedback.last_cmd_vel = feedback->last_cmd_vel;
+  robot_pose_ = feedback->current_pose;
+  goal_handle_.publishFeedback(move_base_feedback);
+
+  // we create a navigation-level oscillation detection using exe_path action's feedback,
+  // as the later doesn't handle oscillations created by quickly failing repeated plans
+
+  // if oscillation detection is enabled by oscillation_timeout != 0
+  if (!oscillation_timeout_.isZero())
+  {
+    // check if oscillating
+    // moved more than the minimum oscillation distance
+    if (mbf_utility::distance(robot_pose_, last_oscillation_pose_) >= oscillation_distance_)
+    {
+      last_oscillation_reset_ = ros::Time::now();
+      last_oscillation_pose_ = robot_pose_;
+
+      if (recovery_trigger_ == OSCILLATING)
+      {
+        ROS_INFO_NAMED("move_base", "Recovered from robot oscillation: restart recovery behaviors");
+        current_recovery_behavior_ = recovery_behaviors_.begin();
+        recovery_trigger_ = NONE;
+      }
+    }
+    else if (last_oscillation_reset_ + oscillation_timeout_ < ros::Time::now())
+    {
+      std::stringstream oscillation_msgs;
+      oscillation_msgs << "Robot is oscillating for " << (ros::Time::now() - last_oscillation_reset_).toSec() << "s!";
+      ROS_WARN_STREAM_NAMED("move_base", oscillation_msgs.str());
+      action_client_exe_path_.cancelGoal();
+
+      if (attemptRecovery())
+      {
+        recovery_trigger_ = OSCILLATING;
+      }
+      else
+      {
+        mbf_msgs::MoveBaseResult move_base_result;
+        move_base_result.outcome = mbf_msgs::MoveBaseResult::OSCILLATION;
+        move_base_result.message = oscillation_msgs.str();
+        move_base_result.final_pose = robot_pose_;
+        move_base_result.angle_to_goal = move_base_feedback.angle_to_goal;
+        move_base_result.dist_to_goal = move_base_feedback.dist_to_goal;
+        goal_handle_.setAborted(move_base_result, move_base_result.message);
+      }
+    }
+  }
+}
+
+void MoveBaseAction::actionGetPathDone(
+    const actionlib::SimpleClientGoalState &state,
+    const mbf_msgs::GetPathResultConstPtr &result_ptr)
+{
+  const mbf_msgs::GetPathResult &get_path_result = *result_ptr;
+  mbf_msgs::MoveBaseResult move_base_result;
+
+  // copy result from get_path action
+  fillMoveBaseResult(get_path_result, move_base_result);
+
+  switch (state.state_)
+  {
+    case actionlib::SimpleClientGoalState::PENDING:
+      ROS_FATAL_STREAM_NAMED("move_base", "get_path PENDING state not implemented, this should not be reachable!");
+      break;
+
+    case actionlib::SimpleClientGoalState::SUCCEEDED:
+      ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
+          << "move_base\" received a path from \""
+          << "get_path\": " << state.getText());
+
+      exe_path_goal_.path = get_path_result.path;
+      ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
+          << "move_base\" sends the path to \""
+          << "exe_path\".");
+
+      if (recovery_trigger_ == GET_PATH)
+      {
+        ROS_WARN_NAMED("move_base", "Recovered from planner failure: restart recovery behaviors");
+        current_recovery_behavior_ = recovery_behaviors_.begin();
+        recovery_trigger_ = NONE;
+      }
+
+      action_client_exe_path_.sendGoal(
+          exe_path_goal_,
+          boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
+          boost::bind(&MoveBaseAction::actionExePathActive, this),
+          boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
+      action_state_ = EXE_PATH;
+      break;
+
+    case actionlib::SimpleClientGoalState::ABORTED:
+
+      if (attemptRecovery())
+      {
+        recovery_trigger_ = GET_PATH;
+      }
+      else
+      {
+        // copy result from get_path action
+        ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << get_path_result.message);
+        goal_handle_.setAborted(move_base_result, state.getText());
+      }
+      action_state_ = FAILED;
+      break;
+
+    case actionlib::SimpleClientGoalState::RECALLED:
+    case actionlib::SimpleClientGoalState::PREEMPTED:
+      ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
+      if (action_state_ == CANCELED)
+      {
+        // move_base preempted while executing get_path; fill result and report canceled to the client
+        ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing get_path");
+        goal_handle_.setCanceled(move_base_result, state.getText());
+      }
+      break;
+
+    case actionlib::SimpleClientGoalState::REJECTED:
+      ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
+      goal_handle_.setCanceled(move_base_result, state.getText());
+      action_state_ = FAILED;
+      break;
+
+    case actionlib::SimpleClientGoalState::LOST:
+      ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"get_path\"!");
+      goal_handle_.setAborted();
+      action_state_ = FAILED;
+      break;
+
+    default:
+      ROS_FATAL_STREAM_NAMED("move_base", "Reached unknown action server state!");
+      goal_handle_.setAborted();
+      action_state_ = FAILED;
+      break;
+  }
+
+  // start replanning if enabled (can be disabled by dynamic reconfigure) and if we started following a path
+  if (!replanning_ || action_state_ != EXE_PATH)
+    return;
+
+  // we reset the replan clock (we can have been stopped for a while) and make a fist sleep, so we don't replan
+  // just after start moving
+  boost::lock_guard<boost::mutex> guard(replanning_mtx_);
+  replanning_rate_.reset();
+  replanning_rate_.sleep();
+  if (!replanning_ || action_state_ != EXE_PATH ||
+      action_client_get_path_.getState() == actionlib::SimpleClientGoalState::PENDING ||
+      action_client_get_path_.getState() == actionlib::SimpleClientGoalState::ACTIVE)
+    return; // another chance to stop replannings after waiting for replanning period
+  ROS_INFO_STREAM_NAMED("move_base", "Start replanning, using the \"get_path\" action!");
+  action_client_get_path_.sendGoal(
+      get_path_goal_,
+      boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
+  );
+}
+
+void MoveBaseAction::actionExePathDone(
+    const actionlib::SimpleClientGoalState &state,
+    const mbf_msgs::ExePathResultConstPtr &result_ptr)
+{
+  ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
+
+  const mbf_msgs::ExePathResult& exe_path_result = *result_ptr;
+  mbf_msgs::MoveBaseResult move_base_result;
+
+  // copy result from exe_path action
+  fillMoveBaseResult(exe_path_result, move_base_result);
+
+  ROS_DEBUG_STREAM_NAMED("move_base", "Current state:" << state.toString());
+
+  switch (state.state_)
+  {
+    case actionlib::SimpleClientGoalState::SUCCEEDED:
+      move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
+      move_base_result.message = "Action \"move_base\" succeeded!";
+      ROS_INFO_STREAM_NAMED("move_base", move_base_result.message);
+      goal_handle_.setSucceeded(move_base_result, move_base_result.message);
+      action_state_ = SUCCEEDED;
+      break;
+
+    case actionlib::SimpleClientGoalState::ABORTED:
+      action_state_ = FAILED;
+
+      switch (exe_path_result.outcome)
+      {
+        case mbf_msgs::ExePathResult::INVALID_PATH:
+        case mbf_msgs::ExePathResult::TF_ERROR:
+        case mbf_msgs::ExePathResult::NOT_INITIALIZED:
+        case mbf_msgs::ExePathResult::INVALID_PLUGIN:
+        case mbf_msgs::ExePathResult::INTERNAL_ERROR:
+          // none of these errors is recoverable
+          goal_handle_.setAborted(move_base_result, state.getText());
+          break;
+
+        default:
+          // all the rest are, so we start calling the recovery behaviors in sequence
+
+          if (attemptRecovery())
+          {
+            recovery_trigger_ = EXE_PATH;
+          }
+          else
+          {
+            ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << exe_path_result.message);
+            goal_handle_.setAborted(move_base_result, state.getText());
+          }
+          break;
+      }
+      break;
+
+    case actionlib::SimpleClientGoalState::RECALLED:
+    case actionlib::SimpleClientGoalState::PREEMPTED:
+      ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
+      if (action_state_ == CANCELED)
+      {
+        // move_base preempted while executing exe_path; fill result and report canceled to the client
+        ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing exe_path");
+        goal_handle_.setCanceled(move_base_result, state.getText());
+      }
+      break;
+
+    case actionlib::SimpleClientGoalState::REJECTED:
+      ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
+      goal_handle_.setCanceled(move_base_result, state.getText());
+      action_state_ = FAILED;
+      break;
+
+    case actionlib::SimpleClientGoalState::LOST:
+      ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"exe_path\"!");
+      goal_handle_.setAborted();
+      action_state_ = FAILED;
+      break;
+
+    default:
+      ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown SimpleActionServer state!");
+      goal_handle_.setAborted();
+      action_state_ = FAILED;
+      break;
+  }
+}
+
+bool MoveBaseAction::attemptRecovery()
+{
+  if (!recovery_enabled_)
+  {
+    ROS_WARN_STREAM_NAMED("move_base", "Recovery behaviors are disabled!");
+    return false;
+  }
+
+  if (current_recovery_behavior_ == recovery_behaviors_.end())
+  {
+    if (recovery_behaviors_.empty())
+    {
+      ROS_WARN_STREAM_NAMED("move_base", "No Recovery Behaviors loaded!");
+    }
+    else
+    {
+      ROS_WARN_STREAM_NAMED("move_base", "Executed all available recovery behaviors!");
+    }
+    return false;
+  }
+
+  recovery_goal_.behavior = *current_recovery_behavior_;
+  ROS_DEBUG_STREAM_NAMED("move_base", "Start recovery behavior\""
+      << *current_recovery_behavior_ <<"\".");
+  action_client_recovery_.sendGoal(
+      recovery_goal_,
+      boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
+  );
+  action_state_ = RECOVERY;
+  return true;
+}
+
+void MoveBaseAction::actionRecoveryDone(
+    const actionlib::SimpleClientGoalState &state,
+    const mbf_msgs::RecoveryResultConstPtr &result_ptr)
+{
+  // give the robot some time to stop oscillating after executing the recovery behavior
+  last_oscillation_reset_ = ros::Time::now();
+
+  const mbf_msgs::RecoveryResult& recovery_result = *result_ptr;
+  mbf_msgs::MoveBaseResult move_base_result;
+
+  // copy result from recovery action
+  fillMoveBaseResult(recovery_result, move_base_result);
+
+  switch (state.state_)
+  {
+    case actionlib::SimpleClientGoalState::REJECTED:
+    case actionlib::SimpleClientGoalState::ABORTED:
+      action_state_ = FAILED;
+
+      ROS_DEBUG_STREAM_NAMED("move_base", "The recovery behavior \""
+          << *current_recovery_behavior_ << "\" has failed. ");
+      ROS_DEBUG_STREAM("Recovery behavior message: " << recovery_result.message
+                                    << ", outcome: " << recovery_result.outcome);
+
+      current_recovery_behavior_++; // use next behavior;
+      if (current_recovery_behavior_ == recovery_behaviors_.end())
+      {
+        ROS_DEBUG_STREAM_NAMED("move_base",
+                               "All recovery behaviors failed. Abort recovering and abort the move_base action");
+        goal_handle_.setAborted(move_base_result, "All recovery behaviors failed.");
+      }
+      else
+      {
+        recovery_goal_.behavior = *current_recovery_behavior_;
+
+        ROS_INFO_STREAM_NAMED("move_base", "Run the next recovery behavior \""
+            << *current_recovery_behavior_ << "\".");
+        action_client_recovery_.sendGoal(
+            recovery_goal_,
+            boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
+        );
+      }
+      break;
+    case actionlib::SimpleClientGoalState::SUCCEEDED:
+      //go to planning state
+      ROS_DEBUG_STREAM_NAMED("move_base", "Execution of the recovery behavior \""
+          << *current_recovery_behavior_ << "\" succeeded!");
+      ROS_DEBUG_STREAM_NAMED("move_base",
+                             "Try planning again and increment the current recovery behavior in the list.");
+      action_state_ = GET_PATH;
+      current_recovery_behavior_++; // use next behavior, the next time;
+      action_client_get_path_.sendGoal(
+          get_path_goal_,
+          boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2)
+      );
+      break;
+    case actionlib::SimpleClientGoalState::RECALLED:
+    case actionlib::SimpleClientGoalState::PREEMPTED:
+      ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"recovery\" has been preempted!");
+      if (action_state_ == CANCELED)
+      {
+        // move_base preempted while executing a recovery; fill result and report canceled to the client
+        ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing a recovery behavior");
+        goal_handle_.setCanceled(move_base_result, state.getText());
+      }
+      break;
+
+    case actionlib::SimpleClientGoalState::LOST:
+      ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"recovery\"!");
+      goal_handle_.setAborted();
+      action_state_ = FAILED;
+      break;
+    default:
+      ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown state!");
+      goal_handle_.setAborted();
+      action_state_ = FAILED;
+      break;
+  }
+}
+
+void MoveBaseAction::actionGetPathReplanningDone(
+    const actionlib::SimpleClientGoalState &state,
+    const mbf_msgs::GetPathResultConstPtr &result)
+{
+  if (!replanning_ || action_state_ != EXE_PATH)
+    return; // replan only while following a path and if replanning is enabled (can be disabled by dynamic reconfigure)
+
+  if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
+  {
+    ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
+    exe_path_goal_.path = result->path;
+    mbf_msgs::ExePathGoal goal(exe_path_goal_);
+    action_client_exe_path_.sendGoal(
+        goal,
+        boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
+        boost::bind(&MoveBaseAction::actionExePathActive, this),
+        boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
+  }
+
+  replanning_mtx_.lock();
+  replanning_rate_.sleep();
+  replanning_mtx_.unlock();
+
+  if (!replanning_ || action_state_ != EXE_PATH)
+    return; // another chance to stop replannings after waiting for replanning period
+
+  ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!");
+  action_client_get_path_.sendGoal(
+      get_path_goal_,
+      boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)); // replanning
+}
+
+} /* namespace mbf_abstract_nav */

navigations/move_base_flex/mbf_abstract_nav/src/planner_action.cpp

@@ -0,0 +1,286 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  planner_action.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include <sstream>
+
+#include "mbf_abstract_nav/planner_action.h"
+
+namespace mbf_abstract_nav
+{
+
+PlannerAction::PlannerAction(
+    const std::string &name,
+    const mbf_utility::RobotInformation &robot_info)
+  : AbstractActionBase(name, robot_info, boost::bind(&mbf_abstract_nav::PlannerAction::run, this, _1, _2)), path_seq_count_(0)
+{
+  ros::NodeHandle private_nh("~");
+  // informative topics: current navigation goal
+  current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
+}
+
+void PlannerAction::run(GoalHandle &goal_handle, AbstractPlannerExecution &execution)
+{
+  const mbf_msgs::GetPathGoal& goal = *(goal_handle.getGoal().get());
+
+  mbf_msgs::GetPathResult result;
+  geometry_msgs::PoseStamped start_pose;
+
+  result.path.header.seq = path_seq_count_++;
+  result.path.header.frame_id = robot_info_.getGlobalFrame();
+
+  double dist_tolerance = goal.tolerance_from_action ? goal.dist_tolerance : 0.0;
+  double angle_tolerance = goal.tolerance_from_action ? goal.angle_tolerance : 0.0;
+  bool use_start_pose = goal.use_start_pose;
+  current_goal_pub_.publish(goal.target_pose);
+
+  bool planner_active = true;
+
+  if (use_start_pose)
+  {
+    start_pose = goal.start_pose;
+    const geometry_msgs::Point& p = start_pose.pose.position;
+    ROS_DEBUG_STREAM_NAMED(name_, "Use the given start pose (" << p.x << ", " << p.y << ", " << p.z << ").");
+  }
+  else
+  {
+    // get the current robot pose
+    if (!robot_info_.getRobotPose(start_pose))
+    {
+      result.outcome = mbf_msgs::GetPathResult::TF_ERROR;
+      result.message = "Could not get the current robot pose!";
+      goal_handle.setAborted(result, result.message);
+      ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
+      return;
+    }
+    else
+    {
+      const geometry_msgs::Point& p = start_pose.pose.position;
+      ROS_DEBUG_STREAM_NAMED(name_, "Got the current robot pose at ("
+          << p.x << ", " << p.y << ", " << p.z << ").");
+    }
+  }
+
+  AbstractPlannerExecution::PlanningState state_planning_input;
+
+  std::vector<geometry_msgs::PoseStamped> plan, global_plan;
+
+  while (planner_active && ros::ok())
+  {
+    // get the current state of the planning thread
+    state_planning_input = execution.getState();
+
+    switch (state_planning_input)
+    {
+      case AbstractPlannerExecution::INITIALIZED:
+        ROS_DEBUG_STREAM_NAMED(name_, "planner state: initialized");
+        if (!execution.start(start_pose, goal.target_pose, dist_tolerance, angle_tolerance))
+        {
+          result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
+          result.message = "Another thread is still planning!";
+          goal_handle.setAborted(result, result.message);
+          ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
+          planner_active = false;
+        }
+        break;
+
+      case AbstractPlannerExecution::STARTED:
+        ROS_DEBUG_STREAM_NAMED(name_, "planner state: started");
+        break;
+
+      case AbstractPlannerExecution::STOPPED:
+        ROS_DEBUG_STREAM_NAMED(name_, "planner state: stopped");
+        ROS_WARN_STREAM_NAMED(name_, "Planning has been stopped rigorously!");
+        result.outcome = mbf_msgs::GetPathResult::STOPPED;
+        result.message = "Global planner has been stopped!";
+        goal_handle.setAborted(result, result.message);
+        planner_active = false;
+        break;
+
+      case AbstractPlannerExecution::CANCELED:
+        ROS_DEBUG_STREAM_NAMED(name_, "planner state: canceled");
+        ROS_DEBUG_STREAM_NAMED(name_, "Global planner has been canceled successfully");
+        result.path.header.stamp = ros::Time::now();
+        result.outcome = mbf_msgs::GetPathResult::CANCELED;
+        result.message = "Global planner has been canceled!";
+        goal_handle.setCanceled(result, result.message);
+        planner_active = false;
+        break;
+
+        // in progress
+      case AbstractPlannerExecution::PLANNING:
+        if (execution.isPatienceExceeded())
+        {
+          ROS_INFO_STREAM_NAMED(name_, "Global planner patience has been exceeded! Cancel planning...");
+          execution.cancel();
+        }
+        else
+        {
+          ROS_DEBUG_THROTTLE_NAMED(2.0, name_, "planner state: planning");
+        }
+        break;
+
+        // found a new plan
+      case AbstractPlannerExecution::FOUND_PLAN:
+        // set time stamp to now
+        result.path.header.stamp = ros::Time::now();
+        plan = execution.getPlan();
+
+        ROS_DEBUG_STREAM_NAMED(name_, "planner state: found plan with cost: " << execution.getCost());
+
+        if (!transformPlanToGlobalFrame(plan, global_plan))
+        {
+          result.outcome = mbf_msgs::GetPathResult::TF_ERROR;
+          result.message = "Could not transform the plan to the global frame!";
+
+          ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
+          goal_handle.setAborted(result, result.message);
+          planner_active = false;
+          break;
+        }
+
+        if (global_plan.empty())
+        {
+          result.outcome = mbf_msgs::GetPathResult::EMPTY_PATH;
+          result.message = "Global planner returned an empty path!";
+
+          ROS_ERROR_STREAM_NAMED(name_, result.message);
+          goal_handle.setAborted(result, result.message);
+          planner_active = false;
+          break;
+        }
+
+        result.path.poses = global_plan;
+        result.cost = execution.getCost();
+        result.outcome = execution.getOutcome();
+        result.message = execution.getMessage();
+        goal_handle.setSucceeded(result, result.message);
+
+        planner_active = false;
+        break;
+
+        // no plan found
+      case AbstractPlannerExecution::NO_PLAN_FOUND:
+        ROS_DEBUG_STREAM_NAMED(name_, "planner state: no plan found");
+        result.outcome = execution.getOutcome();
+        result.message = execution.getMessage();
+        goal_handle.setAborted(result, result.message);
+        planner_active = false;
+        break;
+
+      case AbstractPlannerExecution::MAX_RETRIES:
+        ROS_DEBUG_STREAM_NAMED(name_, "Global planner reached the maximum number of retries");
+        result.outcome = execution.getOutcome();
+        result.message = execution.getMessage();
+        goal_handle.setAborted(result, result.message);
+        planner_active = false;
+        break;
+
+      case AbstractPlannerExecution::PAT_EXCEEDED:
+        ROS_DEBUG_STREAM_NAMED(name_, "Global planner exceeded the patience time");
+        result.outcome = mbf_msgs::GetPathResult::PAT_EXCEEDED;
+        result.message = "Global planner exceeded the patience time";
+        goal_handle.setAborted(result, result.message);
+        planner_active = false;
+        break;
+
+      case AbstractPlannerExecution::INTERNAL_ERROR:
+        ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin!"); // TODO getMessage from planning
+        planner_active = false;
+        result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
+        result.message = "Internal error: Unknown error thrown by the plugin!";
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      default:
+        result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
+        std::ostringstream ss;
+        ss << "Internal error: Unknown state in a move base flex planner execution with the number: "
+           << static_cast<int>(state_planning_input);
+        result.message = ss.str();
+        ROS_FATAL_STREAM_NAMED(name_, result.message);
+        goal_handle.setAborted(result, result.message);
+        planner_active = false;
+    }
+
+
+    if (planner_active)
+    {
+      // try to sleep a bit
+      // normally this thread should be woken up from the planner execution thread
+      // in order to transfer the results to the controller.
+      boost::mutex mutex;
+      boost::unique_lock<boost::mutex> lock(mutex);
+      execution.waitForStateUpdate(boost::chrono::milliseconds(500));
+    }
+  }  // while (planner_active && ros::ok())
+
+  if (!planner_active)
+  {
+    ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
+  }
+  else
+  {
+    ROS_ERROR_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
+  }
+}
+
+bool PlannerAction::transformPlanToGlobalFrame(
+    std::vector<geometry_msgs::PoseStamped> &plan, std::vector<geometry_msgs::PoseStamped> &global_plan)
+{
+  global_plan.clear();
+  std::vector<geometry_msgs::PoseStamped>::iterator iter;
+  bool tf_success = false;
+  for (iter = plan.begin(); iter != plan.end(); ++iter)
+  {
+    geometry_msgs::PoseStamped global_pose;
+    tf_success = mbf_utility::transformPose(robot_info_.getTransformListener(), robot_info_.getGlobalFrame(),
+                                            robot_info_.getTfTimeout(), *iter, global_pose);
+    if (!tf_success)
+    {
+      ROS_ERROR_STREAM("Can not transform pose from the \"" << iter->header.frame_id << "\" frame into the \""
+                                                            << robot_info_.getGlobalFrame() << "\" frame !");
+      return false;
+    }
+    global_plan.push_back(global_pose);
+  }
+  return true;
+}
+
+} /* namespace mbf_abstract_nav */

navigations/move_base_flex/mbf_abstract_nav/src/recovery_action.cpp

@@ -0,0 +1,156 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  recovery_action.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "mbf_abstract_nav/recovery_action.h"
+
+namespace mbf_abstract_nav
+{
+
+RecoveryAction::RecoveryAction(const std::string &name, const mbf_utility::RobotInformation &robot_info)
+  : AbstractActionBase(name, robot_info, boost::bind(&mbf_abstract_nav::RecoveryAction::run, this, _1, _2)){}
+
+void RecoveryAction::run(GoalHandle &goal_handle, AbstractRecoveryExecution &execution)
+{
+  ROS_DEBUG_STREAM_NAMED(name_, "Start action "  << name_);
+
+  const mbf_msgs::RecoveryGoal &goal = *goal_handle.getGoal();
+  mbf_msgs::RecoveryResult result;
+  result.used_plugin = goal.behavior;
+  bool recovery_active = true;
+
+  typename AbstractRecoveryExecution::RecoveryState state_recovery_input;
+
+  while (recovery_active && ros::ok())
+  {
+    state_recovery_input = execution.getState();
+    switch (state_recovery_input)
+    {
+      case AbstractRecoveryExecution::INITIALIZED:
+        ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior \"" << goal.behavior << "\" initialized.");
+        execution.start();
+        break;
+
+      case AbstractRecoveryExecution::STOPPED:
+        ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior stopped rigorously");
+        result.outcome = mbf_msgs::RecoveryResult::STOPPED;
+        result.message = "Recovery has been stopped!";
+        goal_handle.setAborted(result, result.message);
+        recovery_active = false;
+        break;
+
+      case AbstractRecoveryExecution::STARTED:
+        ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior \"" << goal.behavior << "\" was started");
+        break;
+
+      case AbstractRecoveryExecution::RECOVERING:
+
+        if (execution.isPatienceExceeded())
+        {
+          ROS_INFO_STREAM("Recovery behavior \"" << goal.behavior << "\" patience exceeded! Cancel recovering...");
+          execution.cancel();
+        }
+
+        ROS_DEBUG_STREAM_THROTTLE_NAMED(3, name_, "Recovering with: " << goal.behavior);
+        break;
+
+      case AbstractRecoveryExecution::CANCELED:
+        // Recovery behavior supports cancel and it worked
+        recovery_active = false; // stopping the action
+        result.outcome = mbf_msgs::RecoveryResult::CANCELED;
+        result.message = "Recovery behaviour \"" + goal.behavior + "\" canceled!";
+        goal_handle.setCanceled(result, result.message);
+        ROS_DEBUG_STREAM_NAMED(name_, result.message);
+        break;
+
+      case AbstractRecoveryExecution::RECOVERY_DONE:
+        recovery_active = false; // stopping the action
+        result.outcome = execution.getOutcome();
+        result.message = execution.getMessage();
+        if (result.message.empty())
+        {
+          if (result.outcome < 10)
+            result.message = "Recovery \"" + goal.behavior + "\" done";
+          else
+            result.message = "Recovery \"" + goal.behavior + "\" FAILED";
+        }
+
+        ROS_DEBUG_STREAM_NAMED(name_, result.message);
+        goal_handle.setSucceeded(result, result.message);
+        break;
+
+      case AbstractRecoveryExecution::INTERNAL_ERROR:
+        ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin!"); // TODO getMessage from recovery
+        recovery_active = false;
+        result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
+        result.message = "Internal error: Unknown error thrown by the plugin!";
+        goal_handle.setAborted(result, result.message);
+        break;
+
+      default:
+        result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
+        std::stringstream ss;
+        ss << "Internal error: Unknown state in a move base flex recovery execution with the number: "
+           << static_cast<int>(state_recovery_input);
+        result.message = ss.str();
+        ROS_FATAL_STREAM_NAMED(name_, result.message);
+        goal_handle.setAborted(result, result.message);
+        recovery_active = false;
+    }
+
+    if (recovery_active)
+    {
+      // try to sleep a bit
+      // normally the thread should be woken up from the recovery unit
+      // in order to transfer the results to the controller
+      execution.waitForStateUpdate(boost::chrono::milliseconds(500));
+    }
+  }  // while (recovery_active && ros::ok())
+
+  if (!recovery_active)
+  {
+    ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
+  }
+  else
+  {
+    ROS_ERROR_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
+  }
+}
+
+} /* namespace mbf_abstract_nav */