git commit -m "first commit"

navigations/move_base_flex/.gitignore

@@ -0,0 +1,3 @@
+.*
+cmake-build-debug
+!/.gitignore

navigations/move_base_flex/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* 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.
+
+* 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.

navigations/move_base_flex/README.md

@@ -0,0 +1,42 @@
+# Move Base Flex: A Highly Flexible Navigation Framework:
+
+This repository contains Move Base Flex (MBF), a backwards-compatible replacement for move_base. MBF can use existing plugins for move_base, and provides an enhanced version of the same ROS interface. It exposes action servers for planning, controlling and recovering, providing detailed information of the current state and the plugin's feedback. An external executive logic can use MBF and its actions to perform smart and flexible navigation strategies. For example, at [Magazino](https://www.magazino.eu/?lang=en) we have successfully deployed MBF at customer facilities to control TORU robots in highly dynamical environments. Furthermore, MBF enables the use of other map representations, e.g. meshes. The core features are:
+ 
+* Fully backwards-compatible with current ROS navigation.
+* Actions for the submodules planning, controlling and recovering, and services to query the costmaps are provided. This interface allows external executives, e.g. SMACH, or Behavior Trees, to run highly flexible and complex navigation strategies.
+* Comprehensive result and feedback information on all actions, including error codes and messages from the loaded plugins. For users still relying on a unique navigation interface, we have extended move_base action with detailed result and feedback information (though we still provide the current one).
+* Separation between an abstract navigation framework and concrete implementations, allowing faster development of new applications, e.g. 3D navigation.
+* Load multiple planners and controllers, selectable at runtime by setting one of the loaded plugin names in the action goal. 
+* Concurrency: Parallel planning, recovering, controlling by selecting different concurrency slots when defining the action goal. Only different plugins instances can run in parallel.
+
+Please see also the [Move Base Flex Documentation and Tutorials](https://wiki.ros.org/move_base_flex) in the ROS wiki.
+
+## Concepts & Architecture
+
+We have created Move Base Flex for a larger target group besides the standard developers and users of move_base and 2D navigation based on costmaps, as well as addressed move_base's limitations. Since robot navigation can be separated into planning and controlling in many cases, even for outdoor scenarios without the benefits of flat terrain, we designed MBF based on abstract planner-, controller- and recovery behavior-execution classes. To accomplish this goal, we created abstract base classes for the nav core BaseLocalPlanner, BaseGlobalPlanner and RecoveryBehavior plugin interfaces, extending the API to provide a richer and more expressive interface without breaking the current move_base plugin API. The new abstract interfaces allow plugins to return valuable information in each execution cycle, e.g. why a valid plan or a velocity command could not be computed. This information is then passed to the external executive logic through MBF planning, navigation or recovering actions’ feedback and result. The planner, controller and recovery behavior execution is implemented in the abstract execution classes without binding the software implementation to 2D costmaps. In our framework, MoveBase is just a particular implementation of a navigation system: its execution classes implement the abstract ones, bind the system to the costmaps. Thereby, the system can easily be used for other approaches, e.g. navigation on meshes or 3D occupancy grid maps. However, we provide a SimpleNavigationServer class without a binding to costmaps.
+
+MBF architecture:
+![MBF architecture](doc/images/move_base_flex.png)
+
+## Future Work
+MBF is an ongoing project. Some of the improvements that we have planned for the near future are:
+
+* Release MBF Mesh Navigation, see [mesh_navigation](https://github.com/uos/mesh_navigation).
+* Auto select the active controller when having concurrently running controllers
+* Add Ackermann steering API
+* Multi Goal API + Action
+* Add new navigation server and core packages using [grid_map](https://wiki.ros.org/grid_map).
+* Constraints-based goal (see issue https://github.com/magazino/move_base_flex/issues/8)
+
+But, of course you are welcome to propose new fancy features and help make them a reality! Pull Requests are always welcome!
+
+## Build Status
+
+|        | Master | Kinetic | Lunar | Melodic | Noetic |
+|--------|--------|---------|-------|---------|--------|
+| Travis | [![Build Status](https://api.travis-ci.com/magazino/move_base_flex.svg?branch=master)](https://travis-ci.com/magazino/move_base_flex) | [![Build Status](https://api.travis-ci.com/magazino/move_base_flex.svg?branch=kinetic)](https://travis-ci.com/magazino/move_base_flex) | [![Build Status](https://api.travis-ci.com/magazino/move_base_flex.svg?branch=lunar)](https://travis-ci.com/magazino/move_base_flex) | [![Build Status](https://api.travis-ci.com/magazino/move_base_flex.svg?branch=melodic)](https://travis-ci.com/magazino/move_base_flex) | [![Build Status](https://api.travis-ci.com/magazino/move_base_flex.svg?branch=noetic)](https://travis-ci.com/magazino/move_base_flex) |
+| Binary Deb | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__move_base_flex__ubuntu_xenial_amd64__binary)](http://build.ros.org/job/Kbin_uX64__move_base_flex__ubuntu_xenial_amd64__binary/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Lbin_uX64__move_base_flex__ubuntu_xenial_amd64__binary)](http://build.ros.org/job/Lbin_uX64__move_base_flex__ubuntu_xenial_amd64__binary/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__move_base_flex__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__move_base_flex__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__move_base_flex__ubuntu_focal_amd64__binary)](http://build.ros.org/job/Nbin_uF64__move_base_flex__ubuntu_focal_amd64__binary) |
+| Source Deb | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__move_base_flex__ubuntu_xenial__source)](http://build.ros.org/job/Ksrc_uX__move_base_flex__ubuntu_xenial__source/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Lsrc_uX__move_base_flex__ubuntu_xenial__source)](http://build.ros.org/job/Lsrc_uX__move_base_flex__ubuntu_xenial__source/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__move_base_flex__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__move_base_flex__ubuntu_bionic__source/) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__move_base_flex__ubuntu_focal__source)](http://build.ros.org/job/Nsrc_uF__move_base_flex__ubuntu_focal__source/) |
+| Develop | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__move_base_flex__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__move_base_flex__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ldev__move_base_flex__ubuntu_xenial_amd64)](http://build.ros.org/job/Ldev__move_base_flex__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__move_base_flex__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__move_base_flex__ubuntu_bionic_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndev__move_base_flex__ubuntu_focal_amd64)](http://build.ros.org/job/Ndev__move_base_flex__ubuntu_focal_amd64) |
+| Documentation | | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdoc__move_base_flex__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdoc__move_base_flex__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ldoc__move_base_flex__ubuntu_xenial_amd64)](http://build.ros.org/job/Ldoc__move_base_flex__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdoc__move_base_flex__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdoc__move_base_flex__ubuntu_bionic_amd64)| [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndoc__move_base_flex__ubuntu_focal_amd64)](http://build.ros.org/job/Ndoc__move_base_flex__ubuntu_focal_amd64)| 
+

navigations/move_base_flex/mbf_abstract_core/CHANGELOG.rst

@@ -0,0 +1,36 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mbf_abstract_core
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2020-05-25)
+------------------
+
+0.3.1 (2020-04-07)
+------------------
+
+0.3.0 (2020-03-31)
+------------------
+* unify license declaration to BSD-3
+
+0.2.5 (2019-10-11)
+------------------
+
+0.2.4 (2019-06-16)
+------------------
+
+0.2.3 (2018-11-14)
+------------------
+
+0.2.2 (2018-10-10)
+------------------
+
+0.2.1 (2018-10-03)
+------------------
+
+0.2.0 (2018-09-11)
+------------------
+* Update copyright and 3-clause-BSD license
+
+0.1.0 (2018-03-22)
+------------------
+* First release of move_base_flex for kinetic and lunar

navigations/move_base_flex/mbf_abstract_core/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mbf_abstract_core)
+
+find_package(catkin REQUIRED
+        COMPONENTS
+            std_msgs
+            geometry_msgs
+
+        )
+
+catkin_package(
+    INCLUDE_DIRS
+        include
+    CATKIN_DEPENDS
+            std_msgs
+            geometry_msgs
+)
+
+
+## Install project namespaced headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE)

navigations/move_base_flex/mbf_abstract_core/README.md

@@ -0,0 +1,5 @@
+# Move Base Flex Abstract Core {#mainpage}
+
+The [mbf_abstract_core](wiki.ros.org/mbf_abstract_core) package of Move Base Flex (MBF) contains the abstract interfaces for planning controlling and recovering. Derived packages of the [mbf_abstract_nav](wiki.ros.org/mbf_abstract_nav) should use derived interfaces of the [mbf_abstract_core](wiki.ros.org/mbf_abstract_core) which then, for example, contain a method for initialization of the plugin, e.g. by handing over a pointer to the map representation of choice.
+
+The plugins which are loaded by Move Base Flex (a derived package of the mbf_abstract_nav) have to implement derived interfaces of the interfaces which are defined in this package. The mbf_abstract_core::AbstractPlanner provides an interface for a planner plugin to plan global paths. The mbf_abstract_core::AbstractController provides an interface for a controller plugin to control and move a robot. The mbf_abstract_core::AbstractRecovery provides an interface for a recovery plugin to recover the robot in deadlocked situations.

navigations/move_base_flex/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h

@@ -0,0 +1,123 @@
+/*
+ *  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_planner.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_CORE__ABSTRACT_CONTROLLER_H_
+#define MBF_ABSTRACT_CORE__ABSTRACT_CONTROLLER_H_
+
+#include <vector>
+#include <string>
+#include <stdint.h>
+#include <boost/shared_ptr.hpp>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+namespace mbf_abstract_core{
+
+  class AbstractController{
+
+    public:
+
+      typedef boost::shared_ptr< ::mbf_abstract_core::AbstractController > Ptr;
+
+      /**
+       * @brief Destructor
+       */
+      virtual ~AbstractController(){};
+
+      /**
+       * @brief Given the current position, orientation, and velocity of the robot,
+       * compute velocity commands to send to the base.
+       * @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:
+       *         SUCCESS         = 0
+       *         1..9 are reserved as plugin specific non-error results
+       *         FAILURE         = 100   Unspecified failure, only used for old, non-mfb_core based plugins
+       *         CANCELED        = 101
+       *         NO_VALID_CMD    = 102
+       *         PAT_EXCEEDED    = 103
+       *         COLLISION       = 104
+       *         OSCILLATION     = 105
+       *         ROBOT_STUCK     = 106
+       *         MISSED_GOAL     = 107
+       *         MISSED_PATH     = 108
+       *         BLOCKED_PATH    = 109
+       *         INVALID_PATH    = 110
+       *         TF_ERROR        = 111
+       *         NOT_INITIALIZED = 112
+       *         INVALID_PLUGIN  = 113
+       *         INTERNAL_ERROR  = 114
+       *         121..149 are reserved as plugin specific errors
+       */
+      virtual uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose,
+                                               const geometry_msgs::TwistStamped& velocity,
+                                               geometry_msgs::TwistStamped &cmd_vel,
+                                               std::string &message) = 0;
+
+      /**
+       * @brief Check if the goal pose has been achieved by the local planner
+       * @param angle_tolerance The angle tolerance in which the current pose will be partly accepted as reached goal
+       * @param dist_tolerance The distance tolerance in which the current pose will be partly accepted as reached goal
+       * @return True if achieved, false otherwise
+       */
+      virtual bool isGoalReached(double dist_tolerance, double angle_tolerance) = 0;
+
+      /**
+       * @brief Set the plan that the local planner is following
+       * @param plan The plan to pass to the local planner
+       * @return True if the plan was updated successfully, false otherwise
+       */
+      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;
+
+      /**
+       * @brief Requests the planner to cancel, e.g. if it takes too much time.
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel() = 0;
+
+    protected:
+      /**
+       * @brief Constructor
+       */
+      AbstractController(){};
+  };
+} /* namespace mbf_abstract_core */
+
+#endif /* MBF_ABSTRACT_CORE__ABSTRACT_CONTROLLER_H_ */

navigations/move_base_flex/mbf_abstract_core/include/mbf_abstract_core/abstract_planner.h

@@ -0,0 +1,106 @@
+/*
+ *  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_planner.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_CORE__ABSTRACT_PLANNER_H_
+#define MBF_ABSTRACT_CORE__ABSTRACT_PLANNER_H_
+
+#include <vector>
+#include <string>
+#include <stdint.h>
+#include <boost/shared_ptr.hpp>
+#include <geometry_msgs/PoseStamped.h>
+
+namespace mbf_abstract_core
+{
+
+  class AbstractPlanner{
+
+    public:
+      typedef boost::shared_ptr< ::mbf_abstract_core::AbstractPlanner > Ptr;
+
+      /**
+       * @brief Destructor
+       */
+      virtual ~AbstractPlanner(){};
+
+      /**
+       * @brief Given a goal pose in the world, compute a plan.
+       * 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
+       * @param goal The goal pose
+       * @param dist_tolerance Tolerance in meters to the goal position
+       * @param angle_tolerance Tolerance in radians to the goal orientation
+       * @param plan The plan... filled by the planner
+       * @param cost The cost for the the plan
+       * @param message Optional more detailed outcome as a string
+       * @return Result code as described on GetPath action result:
+       *         SUCCESS         = 0
+       *         1..9 are reserved as plugin specific non-error results
+       *         FAILURE         = 50  # Unspecified failure, only used for old, non-mfb_core based plugins
+       *         CANCELED        = 51
+       *         INVALID_START   = 52
+       *         INVALID_GOAL    = 53
+       *         NO_PATH_FOUND   = 54
+       *         PAT_EXCEEDED    = 55
+       *         EMPTY_PATH      = 56
+       *         TF_ERROR        = 57
+       *         NOT_INITIALIZED = 58
+       *         INVALID_PLUGIN  = 59
+       *         INTERNAL_ERROR  = 60
+       *         71..99 are reserved as plugin specific errors
+       */
+      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) = 0;
+
+      /**
+       * @brief Requests the planner to cancel, e.g. if it takes too much time.
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel() = 0;
+
+    protected:
+      /**
+       * @brief Constructor
+       */
+      AbstractPlanner(){};
+  };
+} /* namespace mbf_abstract_core */
+
+#endif /* MBF_ABSTRACT_CORE__ABSTRACT_PLANNER_H_ */

navigations/move_base_flex/mbf_abstract_core/include/mbf_abstract_core/abstract_recovery.h

@@ -0,0 +1,83 @@
+/*
+ *  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_recovery.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_ABSTRACT_CORE__ABSTRACT_RECOVERY_H_
+#define MBF_ABSTRACT_CORE__ABSTRACT_RECOVERY_H_
+
+#include <stdint.h>
+#include <string>
+#include <boost/shared_ptr.hpp>
+
+namespace mbf_abstract_core {
+  /**
+   * @class AbstractRecovery
+   * @brief Provides an interface for recovery behaviors used in navigation.
+   * All recovery behaviors written as plugins for the navigation stack must adhere to this interface.
+   */
+  class AbstractRecovery{
+    public:
+
+      typedef boost::shared_ptr< ::mbf_abstract_core::AbstractRecovery > Ptr;
+
+      /**
+       * @brief Runs the AbstractRecovery
+       * @param message The recovery behavior could set, the message should correspond to the return value
+       * @return An outcome which will be hand over to the action result.
+       */
+      virtual uint32_t runBehavior(std::string& message) = 0;
+
+      /**
+       * @brief Virtual destructor for the interface
+       */
+      virtual ~AbstractRecovery(){}
+
+      /**
+       * @brief Requests the recovery behavior to cancel, e.g. if it takes too much time.
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel() = 0;
+
+    protected:
+      /**
+       * @brief Constructor
+       */
+      AbstractRecovery(){};
+  };
+};  /* namespace mbf_abstract_core */
+
+#endif  /* MBF_ABSTRACT_CORE__ABSTRACT_RECOVERY_H_ */

navigations/move_base_flex/mbf_abstract_core/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+    <name>mbf_abstract_core</name>
+    <version>0.3.2</version>
+    <description>
+        This package provides common interfaces for navigation specific robot actions. It contains the AbstractPlanner, AbstractController and AbstractRecovery plugin interfaces. This interfaces have to be implemented by the plugins to make the plugin available for Move Base Flex. The abstract classes provides a meaningful interface enabling the planners, controllers and recovery behaviors to return information, e.g. why something went wrong. Derivided interfaces can, for example, provide methods to initialize the planner, controller or recovery with map representations like costmap_2d, grid_map or other representations.
+    </description>
+    <url>http://wiki.ros.org/mbf_abstract_core</url>
+    <maintainer email="spuetz@uos.de">Sebastian Pütz</maintainer>
+    <maintainer email="santos@magazino.eu">Jorge Santos</maintainer>
+    <author email="spuetz@uos.de">Sebastian Pütz</author>
+    <author email="santos@magazino.eu">Jorge Santos</author>
+    <license>BSD-3</license>
+    <buildtool_depend>catkin</buildtool_depend>
+    <depend>geometry_msgs</depend>
+    <depend>std_msgs</depend>
+
+    <export>
+      <rosdoc config="rosdoc.yaml" />
+    </export>
+</package>

navigations/move_base_flex/mbf_abstract_core/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 *.rst'

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 */

navigations/move_base_flex/mbf_costmap_core/CHANGELOG.rst

@@ -0,0 +1,38 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mbf_costmap_core
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2020-05-25)
+------------------
+
+0.3.1 (2020-04-07)
+------------------
+
+0.3.0 (2020-03-31)
+------------------
+* unify license declaration to BSD-3
+
+0.2.5 (2019-10-11)
+------------------
+
+0.2.4 (2019-06-16)
+------------------
+
+0.2.3 (2018-11-14)
+------------------
+
+0.2.2 (2018-10-10)
+------------------
+
+0.2.1 (2018-10-03)
+------------------
+* Make MBF melodic and indigo compatible
+* Fix GoalHandle references bug in callbacks
+
+0.2.0 (2018-09-11)
+------------------
+* Concurrency for planners, controllers and recovery behaviors
+
+0.1.0 (2018-03-22)
+------------------
+* First release of move_base_flex for kinetic and lunar

navigations/move_base_flex/mbf_costmap_core/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mbf_costmap_core)
+
+find_package(catkin REQUIRED
+        COMPONENTS
+            std_msgs
+            geometry_msgs
+            mbf_abstract_core
+            mbf_utility
+            tf
+            costmap_2d
+            nav_core
+        )
+
+catkin_package(
+    INCLUDE_DIRS
+        include
+    CATKIN_DEPENDS
+            std_msgs
+            geometry_msgs
+            mbf_abstract_core
+            mbf_utility
+            tf
+            costmap_2d
+            nav_core
+)
+
+## Install project namespaced headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE)

navigations/move_base_flex/mbf_costmap_core/README.md

@@ -0,0 +1,3 @@
+# Move Base Flex Costmap Navigation Core  {#mainpage}
+
+This package provides common interfaces for navigation specific robot actions. It contains the CostmapPlanner, CostmapController and CostmapRecovery interfaces. The interfaces have to be implemented by the plugins to make them available for Move Base Flex using the [mbf_costmap_nav](wiki.ros.org/mbf_costmap_nav) navigation implementation. That implementation inherits the [mbf_abstract_nav](wiki.ros.org/mbf_abstract_nav) implementation and binds the system to a local and a global costmap.

navigations/move_base_flex/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h

@@ -0,0 +1,132 @@
+/*
+ *  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.
+ *
+ *  mbf_costmap_core.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_COSTMAP_CORE__COSTMAP_CONTROLLER_H_
+#define MBF_COSTMAP_CORE__COSTMAP_CONTROLLER_H_
+
+#include <mbf_abstract_core/abstract_controller.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <mbf_utility/types.h>
+
+namespace mbf_costmap_core {
+  /**
+   * @class LocalPlanner
+   * @brief Provides an interface for local planners used in navigation.
+   * All local planners written to work as MBF plugins must adhere to this interface. Alternatively, this
+   * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility.
+   */
+  class CostmapController : public mbf_abstract_core::AbstractController{
+    public:
+
+      typedef boost::shared_ptr< ::mbf_costmap_core::CostmapController > Ptr;
+
+      /**
+       * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands
+       * to send to the base.
+       * @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:
+       *         SUCCESS         = 0
+       *         1..9 are reserved as plugin specific non-error results
+       *         FAILURE         = 100   Unspecified failure, only used for old, non-mfb_core based plugins
+       *         CANCELED        = 101
+       *         NO_VALID_CMD    = 102
+       *         PAT_EXCEEDED    = 103
+       *         COLLISION       = 104
+       *         OSCILLATION     = 105
+       *         ROBOT_STUCK     = 106
+       *         MISSED_GOAL     = 107
+       *         MISSED_PATH     = 108
+       *         BLOCKED_PATH    = 109
+       *         INVALID_PATH    = 110
+       *         TF_ERROR        = 111
+       *         NOT_INITIALIZED = 112
+       *         INVALID_PLUGIN  = 113
+       *         INTERNAL_ERROR  = 114
+       *         121..149 are reserved as plugin specific errors
+       */
+      virtual uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose,
+                                               const geometry_msgs::TwistStamped& velocity,
+                                               geometry_msgs::TwistStamped &cmd_vel,
+                                               std::string &message) = 0;
+
+      /**
+       * @brief Check if the goal pose has been achieved by the local planner within tolerance limits
+       * @remark New on MBF API
+       * @param xy_tolerance Distance tolerance in meters
+       * @param yaw_tolerance Heading tolerance in radians
+       * @return True if achieved, false otherwise
+       */
+      virtual bool isGoalReached(double xy_tolerance, double yaw_tolerance) = 0;
+
+      /**
+       * @brief  Set the plan that the local planner is following
+       * @param plan The plan to pass to the local planner
+       * @return True if the plan was updated successfully, false otherwise
+       */
+      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;
+
+      /**
+       * @brief Requests the planner to cancel, e.g. if it takes too much time
+       * @remark New on MBF API
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel() = 0;
+
+      /**
+       * @brief Constructs the local planner
+       * @param name The name to give this instance of the local planner
+       * @param tf A pointer to a transform listener
+       * @param costmap_ros The cost map to use for assigning costs to local plans
+       */
+      virtual void initialize(std::string name, ::TF *tf, costmap_2d::Costmap2DROS *costmap_ros) = 0;
+
+      /**
+       * @brief  Virtual destructor for the interface
+       */
+      virtual ~CostmapController(){}
+
+    protected:
+      CostmapController(){}
+
+  };
+}  /* namespace mbf_costmap_core */
+
+#endif  /* MBF_COSTMAP_CORE__COSTMAP_CONTROLLER_H_ */

navigations/move_base_flex/mbf_costmap_core/include/mbf_costmap_core/costmap_planner.h

@@ -0,0 +1,113 @@
+/*
+ *  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_global_planner.h
+ *
+ *  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *
+ */
+
+#ifndef MBF_COSTMAP_CORE__COSTMAP_PLANNER_H_
+#define MBF_COSTMAP_CORE__COSTMAP_PLANNER_H_
+
+#include <mbf_abstract_core/abstract_planner.h>
+#include <costmap_2d/costmap_2d_ros.h>
+
+namespace mbf_costmap_core {
+  /**
+   * @class CostmapPlanner
+   * @brief Provides an interface for global planners used in navigation.
+   * All global planners written to work as MBF plugins must adhere to this interface. Alternatively, this
+   * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility.
+   */
+  class CostmapPlanner : public mbf_abstract_core::AbstractPlanner{
+    public:
+
+      typedef boost::shared_ptr< ::mbf_costmap_core::CostmapPlanner > Ptr;
+
+      /**
+       * @brief Given a goal pose in the world, compute a plan
+       * 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
+       * @param goal The goal pose
+       * @param dist_tolerance Tolerance in meters to the goal position
+       * @param angle_tolerance Tolerance in radians to the goal orientation
+       * @param plan The plan... filled by the planner
+       * @param cost The cost for the the plan
+       * @param message Optional more detailed outcome as a string
+       * @return Result code as described on GetPath action result:
+       *         SUCCESS         = 0
+       *         1..9 are reserved as plugin specific non-error results
+       *         FAILURE         = 50  # Unspecified failure, only used for old, non-mfb_core based plugins
+       *         CANCELED        = 51
+       *         INVALID_START   = 52
+       *         INVALID_GOAL    = 53
+       *         NO_PATH_FOUND   = 54
+       *         PAT_EXCEEDED    = 55
+       *         EMPTY_PATH      = 56
+       *         TF_ERROR        = 57
+       *         NOT_INITIALIZED = 58
+       *         INVALID_PLUGIN  = 59
+       *         INTERNAL_ERROR  = 60
+       *         71..99 are reserved as plugin specific errors
+       */
+      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) = 0;
+
+      /**
+       * @brief Requests the planner to cancel, e.g. if it takes too much time.
+       * @remark New on MBF API
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel() = 0;
+
+      /**
+       * @brief Initialization function for the CostmapPlanner
+       * @param name The name of this planner
+       * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
+       */
+      virtual void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) = 0;
+
+      /**
+       * @brief  Virtual destructor for the interface
+       */
+      virtual ~CostmapPlanner(){}
+
+    protected:
+      CostmapPlanner(){}
+
+  };
+}  /* namespace mbf_costmap_core */
+
+#endif  /* MBF_COSTMAP_CORE__COSTMAP_PLANNER_H_ */

navigations/move_base_flex/mbf_costmap_core/include/mbf_costmap_core/costmap_recovery.h

@@ -0,0 +1,94 @@
+/*
+ *  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_global_planner.h
+ *
+ *  author: Sebastian Pütz <spuetz@uniosnabrueck.de>
+ *
+ */
+
+#ifndef MBF_COSTMAP_CORE__COSTMAP_RECOVERY_H_
+#define MBF_COSTMAP_CORE__COSTMAP_RECOVERY_H_
+
+#include <mbf_abstract_core/abstract_recovery.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <mbf_utility/types.h>
+
+namespace mbf_costmap_core
+{
+/**
+ * @class CostmapRecovery
+ * @brief Provides an interface for recovery behaviors used in navigation.
+ * All recovery behaviors written to work as MBF plugins must adhere to this interface. Alternatively, this
+ * class can also operate as a wrapper for old API nav_corebased plugins, providing backward compatibility.
+ */
+class CostmapRecovery : public mbf_abstract_core::AbstractRecovery{
+ public:
+
+  typedef boost::shared_ptr< ::mbf_costmap_core::CostmapRecovery> Ptr;
+
+  /**
+   * @brief Initialization function for the CostmapRecovery
+   * @param tf A pointer to a transform listener
+   * @param global_costmap A pointer to the global_costmap used by the navigation stack
+   * @param local_costmap A pointer to the local_costmap used by the navigation stack
+   */
+  virtual void initialize(std::string name, TF* tf,
+                          costmap_2d::Costmap2DROS* global_costmap,
+                          costmap_2d::Costmap2DROS* local_costmap) = 0;
+
+  /**
+   * @brief Runs the CostmapRecovery
+   * @param message The recovery behavior could set, the message should correspond to the return value
+   * @return An outcome which will be hand over to the action result.
+   */
+  virtual uint32_t runBehavior(std::string& message) = 0;
+
+  /**
+   * @brief Requests the planner to cancel, e.g. if it takes too much time
+   * @remark New on MBF API
+   * @return True if a cancel has been successfully requested, false if not implemented.
+   */
+  virtual bool cancel() = 0;
+
+  /**
+   * @brief Virtual destructor for the interface
+   */
+  virtual ~CostmapRecovery(){}
+
+ protected:
+  CostmapRecovery(){}
+
+};
+}  /* namespace mbf_costmap_core */
+
+#endif /* MBF_COSTMAP_CORE__COSTMAP_RECOVERY_H_ */

navigations/move_base_flex/mbf_costmap_core/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+    <name>mbf_costmap_core</name>
+    <version>0.3.2</version>
+    <description>
+        This package provides common interfaces for navigation specific robot actions. It contains the CostmapPlanner, CostmapController and CostmapRecovery interfaces. The interfaces have to be implemented by the plugins to make them available for Move Base Flex using the mbf_costmap_nav navigation implementation. That implementation inherits the mbf_abstract_nav implementation and binds the system to a local and a global costmap.
+    </description>
+    <url>http://wiki.ros.org/move_base_flex/mbf_costmap_core</url>
+    <maintainer email="spuetz@uos.de">Sebastian Pütz</maintainer>
+    <maintainer email="santos@magazino.eu">Jorge Santos</maintainer>
+    <author email="santos@magazino.eu">Jorge Santos</author>
+    <author email="spuetz@uos.de">Sebastian Pütz</author>
+    <license>BSD-3</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <depend>costmap_2d</depend>
+    <depend>geometry_msgs</depend>
+    <depend>mbf_abstract_core</depend>
+    <depend>mbf_utility</depend>
+    <depend>nav_core</depend>
+    <depend>std_msgs</depend>
+    <depend>tf</depend>
+
+</package>

navigations/move_base_flex/mbf_costmap_nav/CHANGELOG.rst

@@ -0,0 +1,55 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mbf_costmap_nav
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2020-05-25)
+------------------
+* Remove dependency on base_local_planner and move FootprintHelper class to mbf_costmap_nav and make it static
+
+0.3.1 (2020-04-07)
+------------------
+* Ensure that check_costmap_mutex is destroyed after timer.
+* Avoid crash on shutdown by stop shutdown_costmap_timer on destructor
+  and explicitly call the costmap_nav_srv destructor
+
+0.3.0 (2020-03-31)
+------------------
+* add output for cancel method if nav_core plugin is wrapped
+* unify license declaration to BSD-3
+
+0.2.5 (2019-10-11)
+------------------
+* Add clear_on_shutdown functionality
+* 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
+
+0.2.4 (2019-06-16)
+------------------
+* Add check_point_cost service
+* Lock costmaps on clear_costmaps service
+* Replace recursive mutexes with normal ones when not needed
+
+0.2.3 (2018-11-14)
+------------------
+* single publisher for controller execution objects
+
+0.2.2 (2018-10-10)
+------------------
+* Do not use MultiThreadedSpinner, as costmap updates can crash when combining laser scans and point clouds
+* Make start/stop costmaps mutexed, since concurrent calls to start can lead to segfaults
+
+0.2.1 (2018-10-03)
+------------------
+* 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
+
+0.1.0 (2018-03-22)
+------------------
+* First release of move_base_flex for kinetic and lunar

navigations/move_base_flex/mbf_costmap_nav/CMakeLists.txt

@@ -0,0 +1,110 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mbf_costmap_nav)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+  costmap_2d
+  dynamic_reconfigure
+  geometry_msgs
+  mbf_abstract_nav
+  mbf_costmap_core
+  mbf_msgs
+  mbf_utility
+  nav_core
+  nav_msgs
+  roscpp
+  std_msgs
+  std_srvs
+  tf
+)
+
+find_package(Boost COMPONENTS thread chrono REQUIRED)
+
+# dynamic reconfigure
+generate_dynamic_reconfigure_options(
+  cfg/MoveBaseFlex.cfg
+)
+
+set(MBF_NAV_CORE_WRAPPER_LIB mbf_nav_core_wrapper)
+set(MBF_COSTMAP_2D_SERVER_LIB mbf_costmap_server)
+set(MBF_COSTMAP_2D_SERVER_NODE mbf_costmap_nav)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${MBF_COSTMAP_2D_SERVER_LIB}
+  CATKIN_DEPENDS
+  actionlib
+  actionlib_msgs
+  costmap_2d
+  dynamic_reconfigure
+  geometry_msgs
+  mbf_abstract_nav
+  mbf_costmap_core
+  mbf_msgs
+  mbf_utility
+  nav_core
+  nav_msgs
+  pluginlib
+  roscpp
+  std_msgs
+  std_srvs
+  tf
+  DEPENDS Boost
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+add_library(${MBF_NAV_CORE_WRAPPER_LIB}
+  src/nav_core_wrapper/wrapper_global_planner.cpp
+  src/nav_core_wrapper/wrapper_local_planner.cpp
+  src/nav_core_wrapper/wrapper_recovery_behavior.cpp
+)
+add_dependencies(${MBF_NAV_CORE_WRAPPER_LIB} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${MBF_NAV_CORE_WRAPPER_LIB}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+add_library(${MBF_COSTMAP_2D_SERVER_LIB}
+  src/mbf_costmap_nav/costmap_navigation_server.cpp
+  src/mbf_costmap_nav/costmap_planner_execution.cpp
+  src/mbf_costmap_nav/costmap_controller_execution.cpp
+  src/mbf_costmap_nav/costmap_recovery_execution.cpp
+  src/mbf_costmap_nav/costmap_wrapper.cpp
+  src/mbf_costmap_nav/footprint_helper.cpp
+)
+add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${MBF_NAV_CORE_WRAPPER_LIB})
+add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${PROJECT_NAME}_gencfg)
+
+target_link_libraries(${MBF_COSTMAP_2D_SERVER_LIB}
+  ${MBF_NAV_CORE_WRAPPER_LIB}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+add_executable(${MBF_COSTMAP_2D_SERVER_NODE} src/move_base_server_node.cpp)
+add_dependencies(${MBF_COSTMAP_2D_SERVER_NODE} ${MBF_COSTMAP_2D_SERVER_LIB})
+target_link_libraries(${MBF_COSTMAP_2D_SERVER_NODE}
+  ${MBF_COSTMAP_2D_SERVER_LIB}
+  ${catkin_LIBRARIES}
+)
+
+install(TARGETS
+  ${MBF_NAV_CORE_WRAPPER_LIB} ${MBF_COSTMAP_2D_SERVER_LIB} ${MBF_COSTMAP_2D_SERVER_NODE}
+  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}
+)
+
+catkin_install_python(PROGRAMS scripts/move_base_legacy_relay.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

navigations/move_base_flex/mbf_costmap_nav/README.md

@@ -0,0 +1,8 @@
+# Move Base Flex Costmap Navigation Server {#mainpage}
+
+The mbf_costmap_nav package contains the costmap navigation server implementation of Move Base Flex (MBF). The costmap navigation server is bound to the [costmap_2d](http://wiki.ros.org/costmap_2d) 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. 
+
+Additionally the mbf_costmap_nav package comes with a wrapper for the old navigation stack and the plugins which inherits from the [nav_core](http://wiki.ros.org/nav_core) base classes. Preferably it tries to load plugins for the new api, therefore plugins could even support both [move_base](http://wiki.ros.org/move_base) and [move_base_flex](http://wiki.ros.org/move_base_flex) by inheriting both base classes.
+
+
+![mbf_abstract_nav sketch](doc/images/mbf_costmap_nav_s.png)

navigations/move_base_flex/mbf_costmap_nav/cfg/MoveBaseFlex.cfg

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+
+PACKAGE = 'mbf_costmap_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()
+
+# include the abstract configuration common to all MBF-based navigation plus costmap navigation specific parameters
+add_mbf_abstract_nav_params(gen)
+
+gen.add("shutdown_costmaps", bool_t, 0,
+        "Determines whether or not to shutdown the costmaps of the node when move_base_flex is in an inactive state",
+        False)
+gen.add("shutdown_costmaps_delay", double_t, 0,
+        "How long in seconds to wait after last action before shutting down the costmaps", 1.0, 0, 60)
+
+gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)
+
+exit(gen.generate(PACKAGE, "mbf_costmap_nav", "MoveBaseFlex"))

navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_controller_execution.h

@@ -0,0 +1,145 @@
+/*
+ *  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.
+ *
+ *  costmap_controller_execution.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__COSTMAP_CONTROLLER_EXECUTION_H_
+#define MBF_COSTMAP_NAV__COSTMAP_CONTROLLER_EXECUTION_H_
+
+#include <mbf_abstract_nav/abstract_controller_execution.h>
+#include <mbf_costmap_core/costmap_controller.h>
+
+#include "mbf_costmap_nav/MoveBaseFlexConfig.h"
+#include "mbf_costmap_nav/costmap_wrapper.h"
+
+
+namespace mbf_costmap_nav
+{
+/**
+ * @brief The CostmapControllerExecution binds a local costmap to the AbstractControllerExecution and uses the
+ *        nav_core/BaseLocalPlanner class as base plugin interface.
+ * This class makes move_base_flex compatible to the old move_base.
+ *
+ * @ingroup controller_execution move_base_server
+ */
+class CostmapControllerExecution : public mbf_abstract_nav::AbstractControllerExecution
+{
+public:
+
+  /**
+   * @brief Constructor.
+   * @param controller_name Name of the controller to use.
+   * @param controller_ptr Shared pointer to the plugin to use.
+   * @param vel_pub Velocity commands publisher.
+   * @param goal_pub Goal pose publisher (just vor visualization).
+   * @param tf_listener_ptr Shared pointer to a common tf listener.
+   * @param costmap_ptr Shared pointer to the local costmap.
+   * @param config Current server configuration (dynamic).
+   */
+  CostmapControllerExecution(
+      const std::string &controller_name,
+      const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
+      const ros::Publisher &vel_pub,
+      const ros::Publisher &goal_pub,
+      const TFPtr &tf_listener_ptr,
+      const CostmapWrapper::Ptr &costmap_ptr,
+      const MoveBaseFlexConfig &config);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~CostmapControllerExecution();
+
+private:
+
+  /**
+   * @brief Implementation-specific setup function, called right before execution.
+   * This method overrides abstract execution empty implementation with underlying map-specific setup code.
+   */
+  void preRun()
+  {
+    costmap_ptr_->checkActivate();
+  };
+
+  /**
+   * @brief Implementation-specific cleanup function, called right after execution.
+   * This method overrides abstract execution empty implementation with underlying map-specific cleanup code.
+   */
+  void postRun()
+  {
+    costmap_ptr_->checkDeactivate();
+  };
+
+  /**
+   * @brief Implementation-specific safety check, called during execution to ensure it's safe to drive.
+   * This method overrides abstract execution empty implementation with underlying map-specific checks,
+   * more precisely if controller costmap is current.
+   * @return True if costmap is current, false otherwise.
+   */
+  bool safetyCheck();
+
+  /**
+   * @brief Request plugin for a new velocity command. We override this method so we can lock the local costmap
+   *        before calling the planner.
+   * @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 &robot_pose,
+      const geometry_msgs::TwistStamped &robot_velocity,
+      geometry_msgs::TwistStamped &vel_cmd,
+      std::string &message);
+
+  mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config);
+
+  //! Shared pointer to thr local costmap
+  const CostmapWrapper::Ptr &costmap_ptr_;
+
+  //! Whether to lock costmap before calling the controller (see issue #4 for details)
+  bool lock_costmap_;
+
+  //! name of the controller plugin assigned by the class loader
+  std::string controller_name_;
+};
+
+} /* namespace mbf_costmap_nav */
+
+#endif /* MBF_COSTMAP_NAV__COSTMAP_CONTROLLER_EXECUTION_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h

@@ -0,0 +1,268 @@
+/*
+ *  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.
+ *
+ *  costmap_navigation_server.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__COSTMAP_NAVIGATION_SERVER_H_
+#define MBF_COSTMAP_NAV__COSTMAP_NAVIGATION_SERVER_H_
+
+#include <mbf_abstract_nav/abstract_navigation_server.h>
+
+#include <std_srvs/Empty.h>
+#include <mbf_msgs/CheckPath.h>
+#include <mbf_msgs/CheckPose.h>
+#include <mbf_msgs/CheckPoint.h>
+
+#include <nav_core/base_global_planner.h>
+#include <nav_core/base_local_planner.h>
+#include <nav_core/recovery_behavior.h>
+
+#include "mbf_costmap_nav/MoveBaseFlexConfig.h"
+#include "mbf_costmap_nav/costmap_planner_execution.h"
+#include "mbf_costmap_nav/costmap_controller_execution.h"
+#include "mbf_costmap_nav/costmap_recovery_execution.h"
+#include "mbf_costmap_nav/costmap_wrapper.h"
+
+
+namespace mbf_costmap_nav
+{
+/**
+ * @defgroup move_base_server Move Base Server
+ * @brief Classes belonging to the Move Base Server level.
+ */
+
+
+typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> > DynamicReconfigureServerCostmapNav;
+
+/**
+ * @brief The CostmapNavigationServer makes Move Base Flex backwards compatible to the old move_base. It combines the
+ *        execution classes which use the nav_core/BaseLocalPlanner, nav_core/BaseCostmapPlanner and the
+ *        nav_core/RecoveryBehavior base classes as plugin interfaces. These plugin interface are the same for the
+ *        old move_base
+ *
+ * @ingroup navigation_server move_base_server
+ */
+class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServer
+{
+public:
+
+  typedef boost::shared_ptr<CostmapNavigationServer> Ptr;
+
+  /**
+   * @brief Constructor
+   * @param tf_listener_ptr Shared pointer to a common TransformListener
+   */
+  CostmapNavigationServer(const TFPtr &tf_listener_ptr);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~CostmapNavigationServer();
+
+  virtual void stop();
+
+private:
+
+  /**
+   * @brief Create a new 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 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 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 true, if the local planner plugin was successfully loaded.
+   */
+  virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string &planner_type);
+
+  /**
+   * @brief Initializes the controller plugin with its name and pointer to the costmap
+   * @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
+  );
+
+  /**
+   * @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);
+
+  /**
+   * @brief Initializes the controller plugin with its name, a pointer to the TransformListener
+   *        and pointer to the costmap
+   * @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
+  );
+
+  /**
+   * @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);
+
+  /**
+   * @brief Initializes a recovery behavior plugin with its name and pointers to the global and local costmaps
+   * @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);
+
+  /**
+   * @brief Callback method for the check_point_cost service
+   * @param request Request object, see the mbf_msgs/CheckPoint service definition file.
+   * @param response Response object, see the mbf_msgs/CheckPoint service definition file.
+   * @return true, if the service completed successfully, false otherwise
+   */
+  bool callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request,
+                                 mbf_msgs::CheckPoint::Response &response);
+
+  /**
+   * @brief Callback method for the check_pose_cost service
+   * @param request Request object, see the mbf_msgs/CheckPose service definition file.
+   * @param response Response object, see the mbf_msgs/CheckPose service definition file.
+   * @return true, if the service completed successfully, false otherwise
+   */
+  bool callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request,
+                                mbf_msgs::CheckPose::Response &response);
+
+  /**
+   * @brief Callback method for the check_path_cost service
+   * @param request Request object, see the mbf_msgs/CheckPath service definition file.
+   * @param response Response object, see the mbf_msgs/CheckPath service definition file.
+   * @return true, if the service completed successfully, false otherwise
+   */
+  bool callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request,
+                                mbf_msgs::CheckPath::Response &response);
+
+  /**
+   * @brief Callback method for the make_plan service
+   * @param request Empty request object.
+   * @param response Empty response object.
+   * @return true, if the service completed successfully, false otherwise
+   */
+  bool callServiceClearCostmaps(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response);
+
+  /**
+   * @brief Reconfiguration method called by dynamic reconfigure.
+   * @param config Configuration parameters. See the MoveBaseFlexConfig definition.
+   * @param level bit mask, which parameters are set.
+   */
+  void reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level);
+
+  pluginlib::ClassLoader<mbf_costmap_core::CostmapRecovery> recovery_plugin_loader_;
+  pluginlib::ClassLoader<nav_core::RecoveryBehavior> nav_core_recovery_plugin_loader_;
+  pluginlib::ClassLoader<mbf_costmap_core::CostmapController> controller_plugin_loader_;
+  pluginlib::ClassLoader<nav_core::BaseLocalPlanner> nav_core_controller_plugin_loader_;
+  pluginlib::ClassLoader<mbf_costmap_core::CostmapPlanner> planner_plugin_loader_;
+  pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> nav_core_planner_plugin_loader_;
+
+  //! Dynamic reconfigure server for the mbf_costmap2d_specific part
+  DynamicReconfigureServerCostmapNav dsrv_costmap_;
+
+  //! last configuration save
+  mbf_costmap_nav::MoveBaseFlexConfig last_config_;
+
+  //! the default parameter configuration save
+  mbf_costmap_nav::MoveBaseFlexConfig default_config_;
+
+  //! true, if the dynamic reconfigure has been setup
+  bool setup_reconfigure_;
+
+  //! Shared pointer to the common local costmap
+  const CostmapWrapper::Ptr local_costmap_ptr_;
+
+  //! Shared pointer to the common global costmap
+  const CostmapWrapper::Ptr global_costmap_ptr_;
+
+  //! Service Server for the check_point_cost service
+  ros::ServiceServer check_point_cost_srv_;
+
+  //! Service Server for the check_pose_cost service
+  ros::ServiceServer check_pose_cost_srv_;
+
+  //! Service Server for the check_path_cost service
+  ros::ServiceServer check_path_cost_srv_;
+
+  //! Service Server for the clear_costmap service
+  ros::ServiceServer clear_costmaps_srv_;
+};
+
+} /* namespace mbf_costmap_nav */
+
+#endif /* MBF_COSTMAP_NAV__COSTMAP_NAVIGATION_SERVER_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_planner_execution.h

@@ -0,0 +1,137 @@
+/*
+ *  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.
+ *
+ *  costmap_planner_execution.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__COSTMAP_PLANNER_EXECUTION_H_
+#define MBF_COSTMAP_NAV__COSTMAP_PLANNER_EXECUTION_H_
+
+#include <mbf_abstract_nav/abstract_planner_execution.h>
+#include <mbf_costmap_core/costmap_planner.h>
+
+#include "mbf_costmap_nav/MoveBaseFlexConfig.h"
+#include "mbf_costmap_nav/costmap_wrapper.h"
+
+
+namespace mbf_costmap_nav
+{
+/**
+ * @brief The CostmapPlannerExecution binds a global costmap to the AbstractPlannerExecution and uses the
+ *        nav_core/BaseCostmapPlanner class as base plugin interface.
+ * This class makes move_base_flex compatible to the old move_base.
+ *
+ * @ingroup planner_execution move_base_server
+ */
+class CostmapPlannerExecution : public mbf_abstract_nav::AbstractPlannerExecution
+{
+public:
+
+  /**
+   * @brief Constructor.
+   * @param planner_name Name of the planner to use.
+   * @param planner_ptr Shared pointer to the plugin to use.
+   * @param costmap_ptr Shared pointer to the global costmap.
+   * @param config Current server configuration (dynamic).
+   */
+  CostmapPlannerExecution(
+      const std::string &planner_name,
+      const mbf_costmap_core::CostmapPlanner::Ptr &planner_ptr,
+      const CostmapWrapper::Ptr &costmap_ptr,
+      const MoveBaseFlexConfig &config);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~CostmapPlannerExecution();
+
+
+private:
+  /**
+   * @brief Implementation-specific setup function, called right before execution.
+   * This method overrides abstract execution empty implementation with underlying map-specific setup code.
+   */
+  void preRun()
+  {
+    costmap_ptr_->checkActivate();
+  };
+
+  /**
+   * @brief Implementation-specific cleanup function, called right after execution.
+   * This method overrides abstract execution empty implementation with underlying map-specific cleanup code.
+   */
+  void postRun()
+  {
+    costmap_ptr_->checkDeactivate();
+  };
+
+  mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config);
+
+  /**
+   * @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);
+
+  //! Shared pointer to the global planner costmap
+  const CostmapWrapper::Ptr &costmap_ptr_;
+
+  //! Whether to lock costmap before calling the planner (see issue #4 for details)
+  bool lock_costmap_;
+
+  //! Name of the planner assigned by the class loader
+  std::string planner_name_;
+};
+
+} /* namespace mbf_costmap_nav */
+
+#endif /* MBF_COSTMAP_NAV__COSTMAP_PLANNER_EXECUTION_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_recovery_execution.h

@@ -0,0 +1,120 @@
+/*
+ *  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.
+ *
+ *  costmap_recovery_execution.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__COSTMAP_RECOVERY_EXECUTION_H_
+#define MBF_COSTMAP_NAV__COSTMAP_RECOVERY_EXECUTION_H_
+
+#include <mbf_abstract_nav/abstract_recovery_execution.h>
+#include <mbf_costmap_core/costmap_recovery.h>
+
+#include "mbf_costmap_nav/MoveBaseFlexConfig.h"
+#include "mbf_costmap_nav/costmap_wrapper.h"
+
+
+namespace mbf_costmap_nav
+{
+/**
+ * @brief The CostmapRecoveryExecution binds a local and a global costmap to the AbstractRecoveryExecution and uses the
+ *        nav_core/CostmapRecovery class as base plugin interface.
+ * This class makes move_base_flex compatible to the old move_base.
+ *
+ * @ingroup recovery_execution move_base_server
+ */
+class CostmapRecoveryExecution : public mbf_abstract_nav::AbstractRecoveryExecution
+{
+
+public:
+  typedef boost::shared_ptr<CostmapRecoveryExecution> Ptr;
+
+  /**
+   * @brief Constructor.
+   * @param recovery_name Name of the recovery behavior to run.
+   * @param recovery_ptr Shared pointer to the plugin to use.
+   * @param tf_listener_ptr Shared pointer to a common tf listener
+   * @param global_costmap Shared pointer to the global costmap.
+   * @param local_costmap Shared pointer to the local costmap.
+   * @param config Current server configuration (dynamic).
+   */
+  CostmapRecoveryExecution(
+      const std::string &recovery_name,
+      const mbf_costmap_core::CostmapRecovery::Ptr &recovery_ptr,
+      const TFPtr &tf_listener_ptr,
+      const CostmapWrapper::Ptr &global_costmap,
+      const CostmapWrapper::Ptr &local_costmap,
+      const MoveBaseFlexConfig &config);
+
+  /**
+   * Destructor
+   */
+  virtual ~CostmapRecoveryExecution();
+
+private:
+  /**
+   * @brief Implementation-specific setup function, called right before execution.
+   * This method overrides abstract execution empty implementation with underlying map-specific setup code.
+   */
+  void preRun()
+  {
+    local_costmap_->checkActivate();
+    global_costmap_->checkActivate();
+  };
+
+  /**
+   * @brief Implementation-specific cleanup function, called right after execution.
+   * This method overrides abstract execution empty implementation with underlying map-specific cleanup code.
+   */
+  void postRun()
+  {
+    local_costmap_->checkDeactivate();
+    global_costmap_->checkDeactivate();
+  };
+
+  mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config);
+
+  //! Shared pointer to the global costmap
+  const CostmapWrapper::Ptr &global_costmap_;
+
+  //! Shared pointer to thr local costmap
+  const CostmapWrapper::Ptr &local_costmap_;
+};
+
+} /* namespace mbf_costmap_nav */
+
+#endif /* MBF_COSTMAP_NAV__COSTMAP_RECOVERY_EXECUTION_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_wrapper.h

@@ -0,0 +1,120 @@
+/*
+ *  Copyright 2019, 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.
+ *
+ *  costmap_wrapper.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__COSTMAP_WRAPPER_H_
+#define MBF_COSTMAP_NAV__COSTMAP_WRAPPER_H_
+
+#include <costmap_2d/costmap_2d_ros.h>
+
+#include <mbf_utility/types.h>
+
+
+namespace mbf_costmap_nav
+{
+/**
+ * @defgroup move_base_server Move Base Server
+ * @brief Classes belonging to the Move Base Server level.
+ */
+
+
+/**
+ * @brief The CostmapWrapper class manages access to a costmap by locking/unlocking its mutex and handles
+ * (de)activation.
+ *
+ * @ingroup navigation_server move_base_server
+ */
+class CostmapWrapper : public costmap_2d::Costmap2DROS
+{
+public:
+  typedef boost::shared_ptr<CostmapWrapper> Ptr;
+
+  /**
+   * @brief Constructor
+   * @param tf_listener_ptr Shared pointer to a common TransformListener
+   */
+  CostmapWrapper(const std::string &name, const TFPtr &tf_listener_ptr);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~CostmapWrapper();
+
+  /**
+   * @brief Reconfiguration method called by dynamic reconfigure.
+   * @param shutdown_costmap Determines whether or not to shutdown the costmap when move_base_flex is inactive.
+   * @param shutdown_costmap_delay How long in seconds to wait after last action before shutting down the costmap.
+   */
+  void reconfigure(double shutdown_costmap, double shutdown_costmap_delay);
+
+  /**
+   * @brief Clear costmap.
+   */
+  void clear();
+
+  /**
+   * @brief Check whether the costmap should be activated.
+   */
+  void checkActivate();
+
+  /**
+   * @brief Check whether the costmap should and could be deactivated.
+   */
+  void checkDeactivate();
+
+private:
+  /**
+   * @brief Timer-triggered deactivation of the costmap.
+   */
+  void deactivate(const ros::TimerEvent &event);
+
+  //! Private node handle
+  ros::NodeHandle private_nh_;
+
+  boost::mutex check_costmap_mutex_;     //!< Start/stop costmap mutex; concurrent calls to start can lead to segfault
+  bool shutdown_costmap_;                //!< don't update costmap when not using it
+  bool clear_on_shutdown_;               //!< clear the costmap, when shutting down
+  int16_t costmap_users_;                //!< keep track of plugins using costmap
+  ros::Timer shutdown_costmap_timer_;    //!< costmap delayed shutdown timer
+  ros::Duration shutdown_costmap_delay_; //!< costmap delayed shutdown delay
+};
+
+} /* namespace mbf_costmap_nav */
+
+#endif /* MBF_COSTMAP_NAV__COSTMAP_WRAPPER_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h

@@ -0,0 +1,89 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the name of Willow Garage, Inc. 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 OWNER 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.
+ *
+ * Author: TKruse
+ *********************************************************************/
+
+#ifndef FOOTPRINT_HELPER_H_
+#define FOOTPRINT_HELPER_H_
+
+#include <vector>
+
+#include <costmap_2d/costmap_2d.h>
+#include <geometry_msgs/Point.h>
+
+namespace mbf_costmap_nav
+{
+
+struct Cell
+{
+  unsigned int x, y;
+};
+
+class FootprintHelper
+{
+public:
+  /**
+   * @brief  Used to get the cells that make up the footprint of the robot
+   * @param  x The x position of the robot
+   * @param  y The y position of the robot
+   * @param  theta The orientation of the robot
+   * @param  fill If true: returns all cells in the footprint of the robot. If false: returns only the cells that make up the outline of the footprint.
+   * @return The cells that make up either the outline or entire footprint of the robot depending on fill
+   */
+  static std::vector<Cell> getFootprintCells(
+      double x, double y, double theta,
+      std::vector<geometry_msgs::Point> footprint_spec,
+      const costmap_2d::Costmap2D&,
+      bool fill);
+
+  /**
+   * @brief  Use Bresenham's algorithm to trace a line between two points in a grid
+   * @param  x0 The x coordinate of the first point
+   * @param  x1 The x coordinate of the second point
+   * @param  y0 The y coordinate of the first point
+   * @param  y1 The y coordinate of the second point
+   * @param  pts Will be filled with the cells that lie on the line in the grid
+   */
+  static void getLineCells(int x0, int x1, int y0, int y1, std::vector<Cell>& pts);
+
+  /**
+   * @brief  Fill the outline of a polygon, in this case the robot footprint, in a grid
+   * @param  footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint
+   */
+  static void getFillCells(std::vector<Cell>& footprint);
+};
+
+} /* namespace mbf_costmap_nav */
+#endif /* FOOTPRINT_HELPER_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/nav_core_wrapper/wrapper_global_planner.h

@@ -0,0 +1,105 @@
+/*
+ *  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.
+ *
+ *  wrapper_global_planner.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__WRAPPER_GLOBAL_PLANNER_H_
+#define MBF_COSTMAP_NAV__WRAPPER_GLOBAL_PLANNER_H_
+
+#include <nav_core/base_global_planner.h>
+#include "mbf_costmap_core/costmap_planner.h"
+
+namespace mbf_nav_core_wrapper {
+  /**
+   * @class CostmapPlanner
+   * @brief Provides an interface for global planners used in navigation.
+   * All global planners written to work as MBF plugins must adhere to this interface. Alternatively, this
+   * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility.
+   */
+  class WrapperGlobalPlanner : public mbf_costmap_core::CostmapPlanner{
+    public:
+
+      /**
+       * @brief Given a goal pose in the world, compute a plan
+       * 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
+       * @param goal The goal pose
+       * @param dist_tolerance Tolerance in meters to the goal position
+       * @param angle_tolerance Tolerance in radians to the goal orientation
+       * @param plan The plan... filled by the planner
+       * @param cost The cost for the the plan
+       * @param message Optional more detailed outcome as a string
+       * @return Result code as described on GetPath action result, As this is a wrapper to the nav_core,
+       *         only 0 (SUCCESS) and 50 (FAILURE) are supported
+       */
+      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 Requests the planner to cancel, e.g. if it takes too much time.
+       * @remark New on MBF API
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel();
+
+      /**
+       * @brief Initialization function for the CostmapPlanner
+       * @param name The name of this planner
+       * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
+       */
+      virtual void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
+
+      /**
+       * @brief Public constructor used for handling a nav_core-based plugin
+       * @param plugin Backward compatible plugin
+       */
+      WrapperGlobalPlanner(boost::shared_ptr< nav_core::BaseGlobalPlanner > plugin);
+
+      /**
+       * @brief  Virtual destructor for the interface
+       */
+      virtual ~WrapperGlobalPlanner();
+
+    private:
+      boost::shared_ptr< nav_core::BaseGlobalPlanner > nav_core_plugin_;
+  };
+}  /* namespace mbf_nav_core_wrapper */
+
+#endif  /* MBF_COSTMAP_NAV__WRAPPER_GLOBAL_PLANNER_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/nav_core_wrapper/wrapper_local_planner.h

@@ -0,0 +1,127 @@
+/*
+ *  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.
+ *
+ *  wrapper_local_planner.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__WRAPPER_LOCAL_PLANNER_H_
+#define MBF_COSTMAP_NAV__WRAPPER_LOCAL_PLANNER_H_
+
+#include <nav_core/base_local_planner.h>
+#include "mbf_costmap_core/costmap_controller.h"
+
+#include <mbf_utility/types.h>
+
+namespace mbf_nav_core_wrapper {
+  /**
+   * @class LocalPlanner
+   * @brief Provides an interface for local planners used in navigation.
+   * All local planners written to work as MBF plugins must adhere to this interface. Alternatively, this
+   * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility.
+   */
+  class WrapperLocalPlanner : public mbf_costmap_core::CostmapController{
+    public:
+
+      /**
+       * @brief Given the current position, orientation, and velocity of the robot,
+       * compute velocity commands to send to the base
+       * @remark New on MBF API; replaces version without output code and message
+       * @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, as this is a wrapper to the nav_core,
+       *         only 0 (SUCCESS) and 100 (FAILURE) are supported.
+       */
+      virtual uint32_t computeVelocityCommands(
+          const geometry_msgs::PoseStamped &robot_pose,
+          const geometry_msgs::TwistStamped &robot_velocity,
+          geometry_msgs::TwistStamped &cmd_vel,
+          std::string &message);
+
+      /**
+       * @brief Check if the goal pose has been achieved by the local planner
+       * @return True if achieved, false otherwise
+       */
+      virtual bool isGoalReached();
+
+      /**
+       * @brief Check if the goal pose has been achieved by the local planner within tolerance limits
+       * @remark New on MBF API
+       * @param xy_tolerance Distance tolerance in meters
+       * @param yaw_tolerance Heading tolerance in radians
+       * @return True if achieved, false otherwise
+       */
+      virtual bool isGoalReached(double xy_tolerance, double yaw_tolerance);
+
+      /**
+       * @brief  Set the plan that the local planner is following
+       * @param plan The plan to pass to the local planner
+       * @return True if the plan was updated successfully, false otherwise
+       */
+      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan);
+
+      /**
+       * @brief Requests the planner to cancel, e.g. if it takes too much time
+       * @remark New on MBF API
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel();
+
+      /**
+       * @brief Constructs the local planner
+       * @param name The name to give this instance of the local planner
+       * @param tf A pointer to a transform listener
+       * @param costmap_ros The cost map to use for assigning costs to local plans
+       */
+      virtual void initialize(std::string name, TF *tf, costmap_2d::Costmap2DROS *costmap_ros);
+
+      /**
+       * @brief Public constructor used for handling a nav_core-based plugin
+       * @param plugin Backward compatible plugin
+       */
+      WrapperLocalPlanner(boost::shared_ptr< nav_core::BaseLocalPlanner >plugin);
+
+      /**
+       * @brief  Virtual destructor for the interface
+       */
+      virtual ~WrapperLocalPlanner();
+
+    private:
+      boost::shared_ptr< nav_core::BaseLocalPlanner > nav_core_plugin_;
+  };
+}  /* namespace mbf_nav_core_wrapper */
+
+#endif  /* MBF_COSTMAP_NAV__WRAPPER_LOCAL_PLANNER_H_ */

navigations/move_base_flex/mbf_costmap_nav/include/nav_core_wrapper/wrapper_recovery_behavior.h

@@ -0,0 +1,96 @@
+/*
+ *  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.
+ *
+ *  wrapper_recovery_behavior.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_COSTMAP_NAV__WRAPPER_RECOVERY_BEHAVIOR_H_
+#define MBF_COSTMAP_NAV__WRAPPER_RECOVERY_BEHAVIOR_H_
+
+#include <nav_core/recovery_behavior.h>
+#include "mbf_costmap_core/costmap_recovery.h"
+
+#include <mbf_utility/types.h>
+namespace mbf_nav_core_wrapper {
+  /**
+   * @class CostmapRecovery
+   * @brief Provides an interface for recovery behaviors used in navigation.
+   * All recovery behaviors written to work as MBF plugins must adhere to this interface. Alternatively, this
+   * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility.
+   */
+  class WrapperRecoveryBehavior : public mbf_costmap_core::CostmapRecovery{
+    public:
+
+      /**
+       * @brief Initialization function for the CostmapRecovery
+       * @param tf A pointer to a transform listener
+       * @param global_costmap A pointer to the global_costmap used by the navigation stack
+       * @param local_costmap A pointer to the local_costmap used by the navigation stack
+       */
+      virtual void initialize(std::string name, TF* tf,
+                              costmap_2d::Costmap2DROS* global_costmap,
+                              costmap_2d::Costmap2DROS* local_costmap);
+
+      /**
+       * @brief Runs the CostmapRecovery
+       */
+      virtual uint32_t runBehavior(std::string &message);
+
+      /**
+       * @brief Requests the planner to cancel, e.g. if it takes too much time
+       * @remark New on MBF API
+       * @return True if a cancel has been successfully requested, false if not implemented.
+       */
+      virtual bool cancel();
+
+      /**
+       * @brief Public constructor used for handling a nav_core-based plugin
+       * @param plugin Backward compatible plugin
+       */
+      WrapperRecoveryBehavior(boost::shared_ptr< nav_core::RecoveryBehavior > plugin);
+
+      /**
+       * @brief Virtual destructor for the interface
+       */
+      virtual ~WrapperRecoveryBehavior();
+
+    private:
+      boost::shared_ptr< nav_core::RecoveryBehavior > nav_core_plugin_;
+  };
+};  /* namespace mbf_nav_core_wrapper */
+
+#endif  /* MBF_COSTMAP_NAV__WRAPPER_RECOVERY_BEHAVIOR_H_ */

navigations/move_base_flex/mbf_costmap_nav/package.xml

@@ -0,0 +1,41 @@
+<package format="2">
+    <name>mbf_costmap_nav</name>
+    <version>0.3.2</version>
+    <description>
+        The mbf_costmap_nav package contains the costmap navigation server implementation of Move Base Flex (MBF). The costmap navigation server is bound to the <a href="wiki.ros.org/costmap_2d">costmap_2d</a> 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. 
+
+        Additionally the mbf_costmap_nav package comes with a wrapper for the old navigation stack and the plugins which inherits from the <a href="wiki.ros.org/nav_core">nav_core</a> base classes. Preferably it tries to load plugins for the new API. However, plugins could even support both <a href="wiki.ros.org/move_base">move_base</a> and <a href="wiki.ros.org/move_base_flex">move_base_flex</a> by inheriting both base class interfaces located in the <a href="wiki.ros.org/nav_core">nav_core</a> package and in the <a href="mbf_costmap_core">mbf_costmap_core</a> package.
+    </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>
+
+    <depend>actionlib</depend>
+    <depend>actionlib_msgs</depend>
+    <depend>costmap_2d</depend>
+    <depend>dynamic_reconfigure</depend>
+    <depend>geometry_msgs</depend>
+    <depend>mbf_abstract_nav</depend>
+    <depend>mbf_costmap_core</depend>
+    <depend>mbf_msgs</depend>
+    <depend>mbf_utility</depend>
+    <depend>nav_core</depend>
+    <depend>nav_msgs</depend>
+    <depend>pluginlib</depend>
+    <depend>roscpp</depend>
+    <depend>std_msgs</depend>
+    <depend>std_srvs</depend>
+    <depend>tf</depend>
+
+    <!-- Required by the backward compatibility move_base relay -->
+    <exec_depend>move_base</exec_depend>
+    <exec_depend>move_base_msgs</exec_depend>
+
+    <export>
+      <rosdoc config="rosdoc.yaml" />
+    </export>
+</package>

navigations/move_base_flex/mbf_costmap_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_costmap_nav/scripts/move_base_legacy_relay.py

@@ -0,0 +1,144 @@
+#!/usr/bin/env python
+#
+# @author Jorge Santos
+# License: 3-Clause BSD
+
+import actionlib
+import copy
+
+import rospy
+import nav_msgs.srv as nav_srvs
+import mbf_msgs.msg as mbf_msgs
+import move_base_msgs.msg as mb_msgs
+from dynamic_reconfigure.client import Client
+from dynamic_reconfigure.server import Server
+from geometry_msgs.msg import PoseStamped
+from move_base.cfg import MoveBaseConfig
+
+
+"""
+move_base legacy relay node:
+Relays old move_base actions to the new mbf move_base action, similar but with richer result and feedback.
+We also relay the simple goal topic published by RViz, the make_plan service and dynamic reconfiguration
+calls (note that some parameters have changed names; see http://wiki.ros.org/move_base_flex for details)
+"""
+
+# keep configured base local and global planners to send to MBF
+bgp = None
+blp = None
+
+
+def simple_goal_cb(msg):
+    mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg, planner=bgp, controller=blp))
+    rospy.logdebug("Relaying move_base_simple/goal pose to mbf")
+
+
+def mb_execute_cb(msg):
+    mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg.target_pose, planner=bgp, controller=blp),
+                        feedback_cb=mbf_feedback_cb)
+    rospy.logdebug("Relaying legacy move_base goal to mbf")
+    mbf_mb_ac.wait_for_result()
+
+    status = mbf_mb_ac.get_state()
+    result = mbf_mb_ac.get_result()
+
+    rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message)
+    if result.outcome == mbf_msgs.MoveBaseResult.SUCCESS:
+        mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.")
+    else:
+        mb_as.set_aborted(mb_msgs.MoveBaseResult(), result.message)
+
+
+def make_plan_cb(request):
+    mbf_gp_ac.send_goal(mbf_msgs.GetPathGoal(start_pose=request.start, target_pose=request.goal,
+                                             use_start_pose=bool(request.start.header.frame_id), planner=bgp,
+                                             dist_tolerance=request.tolerance, angle_tolerance=request.tolerance,
+                                             tolerance_from_action=bool(request.tolerance > 0.0)))
+    rospy.logdebug("Relaying legacy make_plan service to mbf get_path action server")
+    mbf_gp_ac.wait_for_result()
+
+    status = mbf_gp_ac.get_state()
+    result = mbf_gp_ac.get_result()
+
+    rospy.logdebug("MBF get_path execution completed with result [%d]: %s", result.outcome, result.message)
+    if result.outcome == mbf_msgs.GetPathResult.SUCCESS:
+        return nav_srvs.GetPlanResponse(plan=result.path)
+
+
+def mbf_feedback_cb(feedback):
+    mb_as.publish_feedback(mb_msgs.MoveBaseFeedback(base_position=feedback.current_pose))
+
+
+def mb_reconf_cb(config, level):
+    rospy.logdebug("Relaying legacy move_base reconfigure request to mbf")
+
+    if not hasattr(mb_reconf_cb, "default_config"):
+        mb_reconf_cb.default_config = copy.deepcopy(config)
+
+    if config.get('restore_defaults'):
+        config = mb_reconf_cb.default_config
+
+    mbf_config = copy.deepcopy(config)
+
+    # Map move_base legacy parameters to new mbf ones, and drop those not supported
+    # mbf doesn't allow changing plugins dynamically, but we can provide them in the
+    # action goal, so we keep both base_local_planner and base_global_planner
+    if 'base_local_planner' in mbf_config:
+        global blp
+        blp = mbf_config.pop('base_local_planner')
+    if 'controller_frequency' in mbf_config:
+        mbf_config['controller_frequency'] = mbf_config.pop('controller_frequency')
+    if 'controller_patience' in mbf_config:
+        mbf_config['controller_patience'] = mbf_config.pop('controller_patience')
+    if 'max_controller_retries' in mbf_config:
+        mbf_config['controller_max_retries'] = mbf_config.pop('max_controller_retries')
+    if 'base_global_planner' in mbf_config:
+        global bgp
+        bgp = mbf_config.pop('base_global_planner')
+    if 'planner_frequency' in mbf_config:
+        mbf_config['planner_frequency'] = mbf_config.pop('planner_frequency')
+    if 'planner_patience' in mbf_config:
+        mbf_config['planner_patience'] = mbf_config.pop('planner_patience')
+    if 'max_planning_retries' in mbf_config:
+        mbf_config['planner_max_retries'] = mbf_config.pop('max_planning_retries')
+    if 'recovery_behavior_enabled' in mbf_config:
+        mbf_config['recovery_enabled'] = mbf_config.pop('recovery_behavior_enabled')
+    if 'conservative_reset_dist' in mbf_config:
+        mbf_config.pop('conservative_reset_dist')  # no mbf equivalent for this!
+    if 'clearing_rotation_allowed' in mbf_config:
+        mbf_config.pop('clearing_rotation_allowed')  # no mbf equivalent for this!
+    if 'make_plan_add_unreachable_goal' in mbf_config:
+        mbf_config.pop('make_plan_add_unreachable_goal')  # no mbf equivalent for this!
+    if 'make_plan_clear_costmap' in mbf_config:
+        mbf_config.pop('make_plan_clear_costmap')  # no mbf equivalent for this!
+    mbf_drc.update_configuration(mbf_config)
+    return config
+
+
+if __name__ == '__main__':
+    rospy.init_node("move_base")
+
+    # TODO what happens with malformed target goal???  FAILURE  or INVALID_POSE
+    # txt must be:  "Aborting on goal because it was sent with an invalid quaternion"   
+
+    # move_base_flex get_path and move_base action clients
+    mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction)
+    mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path", mbf_msgs.GetPathAction)
+    mbf_mb_ac.wait_for_server(rospy.Duration(20))
+    mbf_gp_ac.wait_for_server(rospy.Duration(10))
+
+    # move_base_flex dynamic reconfigure client
+    mbf_drc = Client("move_base_flex", timeout=10)
+
+    # move_base simple topic and action server
+    mb_sg = rospy.Subscriber('move_base_simple/goal', PoseStamped, simple_goal_cb)
+    mb_as = actionlib.SimpleActionServer('move_base', mb_msgs.MoveBaseAction, mb_execute_cb, auto_start=False)
+    mb_as.start()
+
+    # move_base make_plan service
+    mb_mps = rospy.Service('~make_plan', nav_srvs.GetPlan, make_plan_cb)
+
+    # move_base dynamic reconfigure server
+    mb_drs = Server(MoveBaseConfig, mb_reconf_cb)
+
+    rospy.spin()

navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp

@@ -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.
+ *
+ *  costmap_controller_execution.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+#include "mbf_costmap_nav/costmap_controller_execution.h"
+
+namespace mbf_costmap_nav
+{
+
+CostmapControllerExecution::CostmapControllerExecution(
+    const std::string &controller_name,
+    const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
+    const ros::Publisher &vel_pub,
+    const ros::Publisher &goal_pub,
+    const TFPtr &tf_listener_ptr,
+    const CostmapWrapper::Ptr &costmap_ptr,
+    const MoveBaseFlexConfig &config)
+      : AbstractControllerExecution(controller_name, controller_ptr, vel_pub, goal_pub,
+                                    tf_listener_ptr, toAbstract(config)),
+        costmap_ptr_(costmap_ptr)
+{
+  ros::NodeHandle private_nh("~");
+  private_nh.param("controller_lock_costmap", lock_costmap_, true);
+}
+
+CostmapControllerExecution::~CostmapControllerExecution()
+{
+}
+
+mbf_abstract_nav::MoveBaseFlexConfig CostmapControllerExecution::toAbstract(const MoveBaseFlexConfig &config)
+{
+  // copy the controller-related abstract configuration common to all MBF-based navigation
+  mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
+  abstract_config.controller_frequency = config.controller_frequency;
+  abstract_config.controller_patience = config.controller_patience;
+  abstract_config.controller_max_retries = config.controller_max_retries;
+  abstract_config.oscillation_timeout = config.oscillation_timeout;
+  abstract_config.oscillation_distance = config.oscillation_distance;
+  return abstract_config;
+}
+
+uint32_t CostmapControllerExecution::computeVelocityCmd(
+    const geometry_msgs::PoseStamped &robot_pose,
+    const geometry_msgs::TwistStamped &robot_velocity,
+    geometry_msgs::TwistStamped &vel_cmd,
+    std::string &message)
+{
+  // Lock the costmap while planning, but following issue #4, we allow to move the responsibility to the planner itself
+  if (lock_costmap_)
+  {
+    boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_ptr_->getCostmap()->getMutex()));
+    return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
+  }
+  return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
+}
+
+bool CostmapControllerExecution::safetyCheck()
+{
+  // Check that the observation buffers for the costmap are current, we don't want to drive blind
+  if (!costmap_ptr_->isCurrent())
+  {
+    ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
+    return false;
+  }
+  return true;
+}
+
+} /* namespace mbf_costmap_nav */

navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp

@@ -0,0 +1,718 @@
+/*
+ *  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.
+ *
+ *  costmap_navigation_server.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include <tf/tf.h>
+#include <nav_msgs/Path.h>
+#include <geometry_msgs/PoseArray.h>
+#include <mbf_msgs/MoveBaseAction.h>
+#include <mbf_abstract_nav/MoveBaseFlexConfig.h>
+#include <actionlib/client/simple_action_client.h>
+#include <nav_core_wrapper/wrapper_global_planner.h>
+#include <nav_core_wrapper/wrapper_local_planner.h>
+#include <nav_core_wrapper/wrapper_recovery_behavior.h>
+
+#include "mbf_costmap_nav/footprint_helper.h"
+#include "mbf_costmap_nav/costmap_navigation_server.h"
+
+namespace mbf_costmap_nav
+{
+
+
+CostmapNavigationServer::CostmapNavigationServer(const TFPtr &tf_listener_ptr) :
+  AbstractNavigationServer(tf_listener_ptr),
+  recovery_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapRecovery"),
+  nav_core_recovery_plugin_loader_("nav_core", "nav_core::RecoveryBehavior"),
+  controller_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapController"),
+  nav_core_controller_plugin_loader_("nav_core", "nav_core::BaseLocalPlanner"),
+  planner_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapPlanner"),
+  nav_core_planner_plugin_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
+  global_costmap_ptr_(new CostmapWrapper("global_costmap", tf_listener_ptr_)),
+  local_costmap_ptr_(new CostmapWrapper("local_costmap", tf_listener_ptr_)),
+  setup_reconfigure_(false)
+{
+  // advertise services and current goal topic
+  check_point_cost_srv_ = private_nh_.advertiseService("check_point_cost",
+                                                       &CostmapNavigationServer::callServiceCheckPointCost, this);
+  check_pose_cost_srv_ = private_nh_.advertiseService("check_pose_cost",
+                                                      &CostmapNavigationServer::callServiceCheckPoseCost, this);
+  check_path_cost_srv_ = private_nh_.advertiseService("check_path_cost",
+                                                      &CostmapNavigationServer::callServiceCheckPathCost, this);
+  clear_costmaps_srv_ = private_nh_.advertiseService("clear_costmaps",
+                                                     &CostmapNavigationServer::callServiceClearCostmaps, this);
+
+  // dynamic reconfigure server for mbf_costmap_nav configuration; also include abstract server parameters
+  dsrv_costmap_ = boost::make_shared<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> >(private_nh_);
+  dsrv_costmap_->setCallback(boost::bind(&CostmapNavigationServer::reconfigure, this, _1, _2));
+
+  // initialize all plugins
+  initializeServerComponents();
+
+  // start all action servers
+  startActionServers();
+}
+
+CostmapNavigationServer::~CostmapNavigationServer()
+{
+  // remove every plugin before its classLoader goes out of scope.
+  controller_plugin_manager_.clearPlugins();
+  planner_plugin_manager_.clearPlugins();
+  recovery_plugin_manager_.clearPlugins();
+
+  action_server_recovery_ptr_.reset();
+  action_server_exe_path_ptr_.reset();
+  action_server_get_path_ptr_.reset();
+  action_server_move_base_ptr_.reset();
+}
+
+mbf_abstract_nav::AbstractPlannerExecution::Ptr CostmapNavigationServer::newPlannerExecution(
+    const std::string &plugin_name,
+    const mbf_abstract_core::AbstractPlanner::Ptr plugin_ptr)
+{
+  return boost::make_shared<mbf_costmap_nav::CostmapPlannerExecution>(
+      plugin_name,
+      boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(plugin_ptr),
+      global_costmap_ptr_,
+      last_config_);
+}
+
+mbf_abstract_nav::AbstractControllerExecution::Ptr CostmapNavigationServer::newControllerExecution(
+    const std::string &plugin_name,
+    const mbf_abstract_core::AbstractController::Ptr plugin_ptr)
+{
+  return boost::make_shared<mbf_costmap_nav::CostmapControllerExecution>(
+      plugin_name,
+      boost::static_pointer_cast<mbf_costmap_core::CostmapController>(plugin_ptr),
+      vel_pub_,
+      goal_pub_,
+      tf_listener_ptr_,
+      local_costmap_ptr_,
+      last_config_);
+}
+
+mbf_abstract_nav::AbstractRecoveryExecution::Ptr CostmapNavigationServer::newRecoveryExecution(
+    const std::string &plugin_name,
+    const mbf_abstract_core::AbstractRecovery::Ptr plugin_ptr)
+{
+  return boost::make_shared<mbf_costmap_nav::CostmapRecoveryExecution>(
+      plugin_name,
+      boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(plugin_ptr),
+      tf_listener_ptr_,
+      global_costmap_ptr_,
+      local_costmap_ptr_,
+      last_config_);
+}
+
+mbf_abstract_core::AbstractPlanner::Ptr CostmapNavigationServer::loadPlannerPlugin(const std::string &planner_type)
+{
+  mbf_abstract_core::AbstractPlanner::Ptr planner_ptr;
+  try
+  {
+    planner_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractPlanner>(
+        planner_plugin_loader_.createInstance(planner_type));
+    std::string planner_name = planner_plugin_loader_.getName(planner_type);
+    ROS_DEBUG_STREAM("mbf_costmap_core-based planner plugin " << planner_name << " loaded.");
+  }
+  catch (const pluginlib::PluginlibException &ex_mbf_core)
+  {
+    ROS_DEBUG_STREAM("Failed to load the " << planner_type << " planner as a mbf_costmap_core-based plugin."
+                                          << " Try to load as a nav_core-based plugin. " << ex_mbf_core.what());
+    try
+    {
+      // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
+      boost::shared_ptr<nav_core::BaseGlobalPlanner> nav_core_planner_ptr = nav_core_planner_plugin_loader_.createInstance(planner_type);
+      planner_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperGlobalPlanner>(nav_core_planner_ptr);
+      std::string planner_name = nav_core_planner_plugin_loader_.getName(planner_type);
+      ROS_DEBUG_STREAM("nav_core-based planner plugin " << planner_name << " loaded");
+    }
+    catch (const pluginlib::PluginlibException &ex_nav_core)
+    {
+      ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it's properly registered"
+          << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
+    }
+  }
+
+  return planner_ptr;
+}
+
+bool CostmapNavigationServer::initializePlannerPlugin(
+    const std::string &name,
+    const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr
+)
+{
+  mbf_costmap_core::CostmapPlanner::Ptr costmap_planner_ptr
+      = boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(planner_ptr);
+  ROS_DEBUG_STREAM("Initialize planner \"" << name << "\".");
+
+  if (!global_costmap_ptr_)
+  {
+    ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
+    return false;
+  }
+
+  costmap_planner_ptr->initialize(name, global_costmap_ptr_.get());
+  ROS_DEBUG("Planner plugin initialized.");
+  return true;
+}
+
+
+mbf_abstract_core::AbstractController::Ptr CostmapNavigationServer::loadControllerPlugin(const std::string &controller_type)
+{
+  mbf_abstract_core::AbstractController::Ptr controller_ptr;
+  try
+  {
+    controller_ptr = controller_plugin_loader_.createInstance(controller_type);
+    std::string controller_name = controller_plugin_loader_.getName(controller_type);
+    ROS_DEBUG_STREAM("mbf_costmap_core-based controller plugin " << controller_name << " loaded.");
+  }
+  catch (const pluginlib::PluginlibException &ex_mbf_core)
+  {
+    ROS_DEBUG_STREAM("Failed to load the " << controller_type << " controller as a mbf_costmap_core-based plugin;"
+                                          << "  we will retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
+    try
+    {
+      // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
+      boost::shared_ptr<nav_core::BaseLocalPlanner> nav_core_controller_ptr
+          = nav_core_controller_plugin_loader_.createInstance(controller_type);
+      controller_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperLocalPlanner>(nav_core_controller_ptr);
+      std::string controller_name = nav_core_controller_plugin_loader_.getName(controller_type);
+      ROS_DEBUG_STREAM("nav_core-based controller plugin " << controller_name << " loaded.");
+    }
+    catch (const pluginlib::PluginlibException &ex_nav_core)
+    {
+      ROS_FATAL_STREAM("Failed to load the " << controller_type << " controller, are you sure it's properly registered"
+          << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
+    }
+  }
+  return controller_ptr;
+}
+
+bool CostmapNavigationServer::initializeControllerPlugin(
+    const std::string &name,
+    const mbf_abstract_core::AbstractController::Ptr &controller_ptr)
+{
+  ROS_DEBUG_STREAM("Initialize controller \"" << name << "\".");
+
+  if (!tf_listener_ptr_)
+  {
+    ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
+    return false;
+  }
+
+  if (!local_costmap_ptr_)
+  {
+    ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
+    return false;
+  }
+
+  mbf_costmap_core::CostmapController::Ptr costmap_controller_ptr
+      = boost::static_pointer_cast<mbf_costmap_core::CostmapController>(controller_ptr);
+  costmap_controller_ptr->initialize(name, tf_listener_ptr_.get(), local_costmap_ptr_.get());
+  ROS_DEBUG_STREAM("Controller plugin \"" << name << "\" initialized.");
+  return true;
+}
+
+mbf_abstract_core::AbstractRecovery::Ptr CostmapNavigationServer::loadRecoveryPlugin(
+    const std::string &recovery_type)
+{
+  mbf_abstract_core::AbstractRecovery::Ptr recovery_ptr;
+
+  try
+  {
+    recovery_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractRecovery>(
+        recovery_plugin_loader_.createInstance(recovery_type));
+    std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
+    ROS_DEBUG_STREAM("mbf_costmap_core-based recovery behavior plugin " << recovery_name << " loaded.");
+  }
+  catch (pluginlib::PluginlibException &ex_mbf_core)
+  {
+    ROS_DEBUG_STREAM("Failed to load the " << recovery_type << " recovery behavior as a mbf_costmap_core-based plugin;"
+        << " Retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
+    try
+    {
+      // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
+      boost::shared_ptr<nav_core::RecoveryBehavior> nav_core_recovery_ptr =
+          nav_core_recovery_plugin_loader_.createInstance(recovery_type);
+
+      recovery_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperRecoveryBehavior>(nav_core_recovery_ptr);
+      std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
+      ROS_DEBUG_STREAM("nav_core-based recovery behavior plugin " << recovery_name << " loaded.");
+
+    }
+    catch (const pluginlib::PluginlibException &ex_nav_core)
+    {
+      ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered"
+          << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
+    }
+  }
+
+  return recovery_ptr;
+}
+
+bool CostmapNavigationServer::initializeRecoveryPlugin(
+    const std::string &name,
+    const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr)
+{
+  ROS_DEBUG_STREAM("Initialize recovery behavior \"" << name << "\".");
+
+  if (!tf_listener_ptr_)
+  {
+    ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
+    return false;
+  }
+
+  if (!local_costmap_ptr_)
+  {
+    ROS_FATAL_STREAM("The local costmap pointer has not been initialized!");
+    return false;
+  }
+
+  if (!global_costmap_ptr_)
+  {
+    ROS_FATAL_STREAM("The global costmap pointer has not been initialized!");
+    return false;
+  }
+
+  mbf_costmap_core::CostmapRecovery::Ptr behavior =
+      boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(behavior_ptr);
+  behavior->initialize(name, tf_listener_ptr_.get(), global_costmap_ptr_.get(), local_costmap_ptr_.get());
+  ROS_DEBUG_STREAM("Recovery behavior plugin \"" << name << "\" initialized.");
+  return true;
+}
+
+
+void CostmapNavigationServer::stop()
+{
+  AbstractNavigationServer::stop();
+  ROS_INFO_STREAM_NAMED("mbf_costmap_nav", "Stopping local and global costmap for shutdown");
+  local_costmap_ptr_->stop();
+  global_costmap_ptr_->stop();
+}
+
+void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level)
+{
+  // 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;
+  }
+
+  // fill the abstract configuration common to all MBF-based navigation
+  mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
+  abstract_config.planner_frequency = config.planner_frequency;
+  abstract_config.planner_patience = config.planner_patience;
+  abstract_config.planner_max_retries = config.planner_max_retries;
+  abstract_config.controller_frequency = config.controller_frequency;
+  abstract_config.controller_patience = config.controller_patience;
+  abstract_config.controller_max_retries = config.controller_max_retries;
+  abstract_config.recovery_enabled = config.recovery_enabled;
+  abstract_config.recovery_patience = config.recovery_patience;
+  abstract_config.oscillation_timeout = config.oscillation_timeout;
+  abstract_config.oscillation_distance = config.oscillation_distance;
+  abstract_config.restore_defaults = config.restore_defaults;
+  mbf_abstract_nav::AbstractNavigationServer::reconfigure(abstract_config, level);
+
+  // also reconfigure costmaps
+  local_costmap_ptr_->reconfigure(config.shutdown_costmaps, config.shutdown_costmaps_delay);
+  global_costmap_ptr_->reconfigure(config.shutdown_costmaps, config.shutdown_costmaps_delay);
+
+  last_config_ = config;
+}
+
+bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request,
+                                                        mbf_msgs::CheckPoint::Response &response)
+{
+  // selecting the requested costmap
+  CostmapWrapper::Ptr costmap;
+  std::string costmap_name;
+  switch (request.costmap)
+  {
+    case mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP:
+      costmap = local_costmap_ptr_;
+      costmap_name = "local costmap";
+      break;
+    case mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP:
+      costmap = global_costmap_ptr_;
+      costmap_name = "global costmap";
+      break;
+    default:
+      ROS_ERROR_STREAM("No valid costmap provided; options are "
+                           << mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP << ": local costmap, "
+                           << mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP << ": global costmap");
+      return false;
+  }
+
+  // get target point as x, y coordinates
+  std::string costmap_frame = costmap->getGlobalFrameID();
+
+  geometry_msgs::PointStamped point;
+  if (!mbf_utility::transformPoint(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.point, point))
+  {
+    ROS_ERROR_STREAM("Transform target point to " << costmap_name << " frame '" << costmap_frame << "' failed");
+    return false;
+  }
+
+  double x = point.point.x;
+  double y = point.point.y;
+
+  // ensure costmap is active so cost reflects latest sensor readings
+  costmap->checkActivate();
+  unsigned int mx, my;
+  if (!costmap->getCostmap()->worldToMap(x, y, mx, my))
+  {
+    // point is outside of the map
+    response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::OUTSIDE);
+    ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is outside the map (cost = " << response.cost << ")");
+  }
+  else
+  {
+    // lock costmap so content doesn't change while checking cell costs
+    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
+
+    // get cost of cell under point and classify as one of the states: UNKNOWN, LETHAL, INSCRIBED, FREE
+    response.cost = costmap->getCostmap()->getCost(mx, my);
+    switch (response.cost)
+    {
+      case costmap_2d::NO_INFORMATION:
+        response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::UNKNOWN);
+        ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in unknown space! (cost = " << response.cost << ")");
+        break;
+      case costmap_2d::LETHAL_OBSTACLE:
+        response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::LETHAL);
+        ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in collision! (cost = " << response.cost << ")");
+        break;
+      case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
+        response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::INSCRIBED);
+        ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is near an obstacle (cost = " << response.cost << ")");
+        break;
+      default:
+        response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::FREE);
+        ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is free (cost = " << response.cost << ")");
+        break;
+    }
+  }
+
+  costmap->checkDeactivate();
+  return true;
+}
+
+bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request,
+                                                       mbf_msgs::CheckPose::Response &response)
+{
+  // selecting the requested costmap
+  CostmapWrapper::Ptr costmap;
+  std::string costmap_name;
+  switch (request.costmap)
+  {
+    case mbf_msgs::CheckPose::Request::LOCAL_COSTMAP:
+      costmap = local_costmap_ptr_;
+      costmap_name = "local costmap";
+      break;
+    case mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP:
+      costmap = global_costmap_ptr_;
+      costmap_name = "global costmap";
+      break;
+    default:
+      ROS_ERROR_STREAM("No valid costmap provided; options are "
+                       << mbf_msgs::CheckPose::Request::LOCAL_COSTMAP << ": local costmap, "
+                       << mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP << ": global costmap");
+      return false;
+  }
+
+  // get target pose or current robot pose as x, y, yaw coordinates
+  std::string costmap_frame = costmap->getGlobalFrameID();
+
+  geometry_msgs::PoseStamped pose;
+  if (request.current_pose)
+  {
+    if (!mbf_utility::getRobotPose(*tf_listener_ptr_, robot_frame_, costmap_frame, ros::Duration(0.5), pose))
+    {
+      ROS_ERROR_STREAM("Get robot pose on " << costmap_name << " frame '" << costmap_frame << "' failed");
+      return false;
+    }
+  }
+  else
+  {
+    if (!mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.pose, pose))
+    {
+      ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
+      return false;
+    }
+  }
+
+  double x = pose.pose.position.x;
+  double y = pose.pose.position.y;
+  double yaw = tf::getYaw(pose.pose.orientation);
+
+  // ensure costmap is active so cost reflects latest sensor readings
+  costmap->checkActivate();
+
+  // pad raw footprint to the requested safety distance; note that we discard footprint_padding parameter effect
+  std::vector<geometry_msgs::Point> footprint = costmap->getUnpaddedRobotFootprint();
+  costmap_2d::padFootprint(footprint, request.safety_dist);
+
+  // use footprint helper to get all the cells totally or partially within footprint polygon
+  std::vector<Cell> footprint_cells =
+    FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true);
+  response.state = mbf_msgs::CheckPose::Response::FREE;
+  if (footprint_cells.empty())
+  {
+    // no cells within footprint polygon must mean that robot is completely outside of the map
+    response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::OUTSIDE));
+  }
+  else
+  {
+    // lock costmap so content doesn't change while adding cell costs
+    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
+
+    // integrate the cost of all cells; state value precedence is UNKNOWN > LETHAL > INSCRIBED > FREE
+    for (int i = 0; i < footprint_cells.size(); ++i)
+    {
+      unsigned char cost = costmap->getCostmap()->getCost(footprint_cells[i].x, footprint_cells[i].y);
+      switch (cost)
+      {
+        case costmap_2d::NO_INFORMATION:
+          response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
+          response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
+          break;
+        case costmap_2d::LETHAL_OBSTACLE:
+          response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::LETHAL));
+          response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
+          break;
+        case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
+          response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::INSCRIBED));
+          response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
+          break;
+        default:
+          response.cost += cost;
+          break;
+      }
+    }
+  }
+
+  // Provide some details of the outcome
+  switch (response.state)
+  {
+    case mbf_msgs::CheckPose::Response::OUTSIDE:
+      ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is outside the map (cost = " << response.cost
+                                << "; safety distance = " << request.safety_dist << ")");
+      break;
+    case mbf_msgs::CheckPose::Response::UNKNOWN:
+      ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in unknown space! (cost = " << response.cost
+                                << "; safety distance = " << request.safety_dist << ")");
+      break;
+    case mbf_msgs::CheckPose::Response::LETHAL:
+      ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in collision! (cost = " << response.cost
+                                << "; safety distance = " << request.safety_dist << ")");
+      break;
+    case mbf_msgs::CheckPose::Response::INSCRIBED:
+      ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is near an obstacle (cost = " << response.cost
+                                << "; safety distance = " << request.safety_dist << ")");
+      break;
+    case mbf_msgs::CheckPose::Response::FREE:
+      ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is free (cost = " << response.cost
+                                << "; safety distance = " << request.safety_dist << ")");
+      break;
+  }
+
+  costmap->checkDeactivate();
+  return true;
+}
+
+bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request,
+                                                       mbf_msgs::CheckPath::Response &response)
+{
+  // selecting the requested costmap
+  CostmapWrapper::Ptr costmap;
+  std::string costmap_name;
+  switch (request.costmap)
+  {
+    case mbf_msgs::CheckPath::Request::LOCAL_COSTMAP:
+      costmap = local_costmap_ptr_;
+      costmap_name = "local costmap";
+      break;
+    case mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP:
+      costmap = global_costmap_ptr_;
+      costmap_name = "global costmap";
+      break;
+    default:
+      ROS_ERROR_STREAM("No valid costmap provided; options are "
+                       << mbf_msgs::CheckPath::Request::LOCAL_COSTMAP << ": local costmap, "
+                       << mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP << ": global costmap");
+      return false;
+  }
+
+  // ensure costmap is active so cost reflects latest sensor readings
+  costmap->checkActivate();
+
+  // get target pose or current robot pose as x, y, yaw coordinates
+  std::string costmap_frame = costmap->getGlobalFrameID();
+
+  std::vector<geometry_msgs::Point> footprint;
+  if (!request.path_cells_only)
+  {
+    // unless we want to check just the cells directly traversed by the path, pad raw footprint
+    // to the requested safety distance; note that we discard footprint_padding parameter effect
+    footprint = costmap->getUnpaddedRobotFootprint();
+    costmap_2d::padFootprint(footprint, request.safety_dist);
+  }
+
+  // lock costmap so content doesn't change while adding cell costs
+  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
+
+  geometry_msgs::PoseStamped pose;
+
+  response.state = mbf_msgs::CheckPath::Response::FREE;
+
+  for (int i = 0; i < request.path.poses.size(); ++i)
+  {
+    response.last_checked = i;
+
+    if (!mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.path.poses[i], pose))
+    {
+      ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
+      return false;
+    }
+
+    double x = pose.pose.position.x;
+    double y = pose.pose.position.y;
+    double yaw = tf::getYaw(pose.pose.orientation);
+    std::vector<Cell> cells_to_check;
+    if (request.path_cells_only)
+    {
+      Cell cell;
+      if (costmap->getCostmap()->worldToMap(x, y, cell.x, cell.y))
+      {
+        cells_to_check.push_back(cell);  // out of map if false; cells_to_check will be empty
+      }
+    }
+    else
+    {
+      // use footprint helper to get all the cells totally or partially within footprint polygon
+      cells_to_check = FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true);
+    }
+
+    if (cells_to_check.empty())
+    {
+      // if path_cells_only is true, this means that current path's pose is outside the map
+      // if not, no cells within footprint polygon means that robot is completely outside of the map
+      response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::OUTSIDE));
+    }
+    else
+    {
+      // integrate the cost of all cells; state value precedence is UNKNOWN > LETHAL > INSCRIBED > FREE
+      // we apply the requested cost multipliers if different from zero (default value)
+      for (int j = 0; j < cells_to_check.size(); ++j)
+      {
+        unsigned char cost = costmap->getCostmap()->getCost(cells_to_check[j].x, cells_to_check[j].y);
+        switch (cost)
+        {
+          case costmap_2d::NO_INFORMATION:
+            response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
+            response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
+            break;
+          case costmap_2d::LETHAL_OBSTACLE:
+            response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::LETHAL));
+            response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
+            break;
+          case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
+            response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::INSCRIBED));
+            response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
+            break;
+          default:response.cost += cost;
+            break;
+        }
+      }
+    }
+
+    if (request.return_on && response.state >= request.return_on)
+    {
+      // i-th pose state is bad enough for the client, so provide some details of the outcome and abort checking
+      switch (response.state)
+      {
+        case mbf_msgs::CheckPath::Response::OUTSIDE:
+          ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes outside the map "
+                           << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
+          break;
+        case mbf_msgs::CheckPath::Response::UNKNOWN:
+          ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in unknown space! "
+                           << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
+          break;
+        case mbf_msgs::CheckPath::Response::LETHAL:
+          ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in collision! "
+                           << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
+          break;
+        case mbf_msgs::CheckPath::Response::INSCRIBED:
+          ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes near an obstacle "
+                           << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
+          break;
+        case mbf_msgs::CheckPath::Response::FREE:
+          ROS_DEBUG_STREAM("Path is entirely free (maximum cost = "
+                           << response.cost << "; safety distance = " << request.safety_dist << ")");
+          break;
+      }
+
+      break;
+    }
+
+    i += request.skip_poses;  // skip some poses to speedup processing (disabled by default)
+  }
+
+  costmap->checkDeactivate();
+  return true;
+}
+
+bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request &request,
+                                                       std_srvs::Empty::Response &response)
+{
+  // clear both costmaps
+  local_costmap_ptr_->clear();
+  global_costmap_ptr_->clear();
+  return true;
+}
+
+} /* namespace mbf_costmap_nav */

navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_planner_execution.cpp

@@ -0,0 +1,88 @@
+/*
+ *  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.
+ *
+ *  costmap_planner_execution.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+#include <nav_core_wrapper/wrapper_global_planner.h>
+
+#include "mbf_costmap_nav/costmap_planner_execution.h"
+
+namespace mbf_costmap_nav
+{
+
+CostmapPlannerExecution::CostmapPlannerExecution(
+    const std::string &planner_name,
+    const mbf_costmap_core::CostmapPlanner::Ptr &planner_ptr,
+    const CostmapWrapper::Ptr &costmap_ptr,
+    const MoveBaseFlexConfig &config)
+      : AbstractPlannerExecution(planner_name, planner_ptr, toAbstract(config)),
+        costmap_ptr_(costmap_ptr)
+{
+  ros::NodeHandle private_nh("~");
+  private_nh.param("planner_lock_costmap", lock_costmap_, true);
+}
+
+CostmapPlannerExecution::~CostmapPlannerExecution()
+{
+}
+
+mbf_abstract_nav::MoveBaseFlexConfig CostmapPlannerExecution::toAbstract(const MoveBaseFlexConfig &config)
+{
+  // copy the planner-related abstract configuration common to all MBF-based navigation
+  mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
+  abstract_config.planner_frequency = config.planner_frequency;
+  abstract_config.planner_patience = config.planner_patience;
+  abstract_config.planner_max_retries = config.planner_max_retries;
+  return abstract_config;
+}
+
+uint32_t CostmapPlannerExecution::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)
+{
+  if (lock_costmap_)
+  {
+    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_ptr_->getCostmap()->getMutex()));
+    return planner_->makePlan(start, goal, dist_tolerance, angle_tolerance, plan, cost, message);
+  }
+  return planner_->makePlan(start, goal, dist_tolerance, angle_tolerance, plan, cost, message);
+}
+
+} /* namespace mbf_costmap_nav */

navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_recovery_execution.cpp

@@ -0,0 +1,72 @@
+/*
+ *  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.
+ *
+ *  costmap_recovery_execution.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+#include <nav_core/recovery_behavior.h>
+#include "nav_core_wrapper/wrapper_recovery_behavior.h"
+#include "mbf_costmap_nav/costmap_recovery_execution.h"
+
+namespace mbf_costmap_nav
+{
+
+CostmapRecoveryExecution::CostmapRecoveryExecution(
+    const std::string &recovery_name,
+    const mbf_costmap_core::CostmapRecovery::Ptr &recovery_ptr,
+    const TFPtr &tf_listener_ptr,
+    const CostmapWrapper::Ptr &global_costmap,
+    const CostmapWrapper::Ptr &local_costmap,
+    const MoveBaseFlexConfig &config)
+      : AbstractRecoveryExecution(recovery_name, recovery_ptr, tf_listener_ptr, toAbstract(config)),
+        global_costmap_(global_costmap), local_costmap_(local_costmap)
+{
+}
+
+CostmapRecoveryExecution::~CostmapRecoveryExecution()
+{
+}
+
+mbf_abstract_nav::MoveBaseFlexConfig CostmapRecoveryExecution::toAbstract(const MoveBaseFlexConfig &config)
+{
+  // copy the recovery-related abstract configuration common to all MBF-based navigation
+  mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
+  abstract_config.recovery_enabled = config.recovery_enabled;
+  abstract_config.recovery_patience = config.recovery_patience;
+  return abstract_config;
+}
+
+} /* namespace mbf_costmap_nav */

navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp

@@ -0,0 +1,139 @@
+/*
+ *  Copyright 2019, 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.
+ *
+ *  costmap_wrapper.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "mbf_costmap_nav/costmap_wrapper.h"
+
+
+namespace mbf_costmap_nav
+{
+
+
+CostmapWrapper::CostmapWrapper(const std::string &name, const TFPtr &tf_listener_ptr) :
+  costmap_2d::Costmap2DROS(name, *tf_listener_ptr),
+  shutdown_costmap_(false), costmap_users_(0), private_nh_("~")
+{
+  // even if shutdown_costmaps is a dynamically reconfigurable parameter, we
+  // need it here to decide whether to start or not the costmap on starting up
+  private_nh_.param("shutdown_costmaps", shutdown_costmap_, false);
+  private_nh_.param("clear_on_shutdown", clear_on_shutdown_, false);
+
+  if (shutdown_costmap_)
+    // initialize costmap stopped if shutdown_costmaps parameter is true
+    stop();
+  else
+    // otherwise costmap_users_ is at least 1, as costmap is always active
+    ++costmap_users_;
+}
+
+CostmapWrapper::~CostmapWrapper()
+{
+  shutdown_costmap_timer_.stop();
+}
+
+
+void CostmapWrapper::reconfigure(double shutdown_costmap, double shutdown_costmap_delay)
+{
+  shutdown_costmap_delay_ = ros::Duration(shutdown_costmap_delay);
+  if (shutdown_costmap_delay_.isZero())
+    ROS_WARN("Zero shutdown costmaps delay is not recommended, as it forces us to enable costmaps on each action");
+
+  if (shutdown_costmap_ && !shutdown_costmap)
+  {
+    checkActivate();
+    shutdown_costmap_ = shutdown_costmap;
+  }
+  if (!shutdown_costmap_ && shutdown_costmap)
+  {
+    shutdown_costmap_ = shutdown_costmap;
+    checkDeactivate();
+  }
+}
+
+void CostmapWrapper::clear()
+{
+  // lock and clear costmap
+  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*getCostmap()->getMutex());
+  resetLayers();
+}
+
+void CostmapWrapper::checkActivate()
+{
+  boost::mutex::scoped_lock sl(check_costmap_mutex_);
+
+  shutdown_costmap_timer_.stop();
+
+  // Activate costmap if we shutdown them when not moving and they are not already active. This method must be
+  // synchronized because start costmap can take up to 1/update freq., and concurrent calls to it can lead to segfaults
+  if (shutdown_costmap_ && !costmap_users_)
+  {
+    start();
+    ROS_DEBUG_STREAM("" << name_ << " activated");
+  }
+  ++costmap_users_;
+}
+
+void CostmapWrapper::checkDeactivate()
+{
+  boost::mutex::scoped_lock sl(check_costmap_mutex_);
+
+  --costmap_users_;
+  ROS_ASSERT_MSG(costmap_users_ >= 0, "Negative number (%d) of active users count!", costmap_users_);
+  if (shutdown_costmap_ && !costmap_users_)
+  {
+    // Delay costmap shutdown by shutdown_costmap_delay so we don't need to enable at each step of a normal
+    // navigation sequence, what is terribly inefficient; the timer is stopped on costmap re-activation and
+    // reset after every new call to deactivate
+    shutdown_costmap_timer_ =
+      private_nh_.createTimer(shutdown_costmap_delay_, &CostmapWrapper::deactivate, this, true);
+  }
+}
+
+void CostmapWrapper::deactivate(const ros::TimerEvent &event)
+{
+  boost::mutex::scoped_lock sl(check_costmap_mutex_);
+
+  ROS_ASSERT_MSG(!costmap_users_, "Deactivating costmap with %d active users!", costmap_users_);
+  if (clear_on_shutdown_)
+    clear();  // do before stop, as some layers (e.g. obstacle and voxel) reactivate their subscribers on reset
+  stop();
+  ROS_DEBUG_STREAM("" << name_ << " deactivated");
+}
+
+} /* namespace mbf_costmap_nav */

navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp

@@ -0,0 +1,237 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the name of Willow Garage, Inc. 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 OWNER 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.
+ *
+ * Author: TKruse
+ *********************************************************************/
+
+#include "mbf_costmap_nav/footprint_helper.h"
+
+namespace mbf_costmap_nav
+{
+
+void FootprintHelper::getLineCells(int x0, int x1, int y0, int y1, std::vector<Cell>& pts) {
+  //Bresenham Ray-Tracing
+  int deltax = abs(x1 - x0);        // The difference between the x's
+  int deltay = abs(y1 - y0);        // The difference between the y's
+  int x = x0;                       // Start x off at the first pixel
+  int y = y0;                       // Start y off at the first pixel
+
+  int xinc1, xinc2, yinc1, yinc2;
+  int den, num, numadd, numpixels;
+
+  Cell pt;
+
+  if (x1 >= x0)                 // The x-values are increasing
+  {
+    xinc1 = 1;
+    xinc2 = 1;
+  }
+  else                          // The x-values are decreasing
+  {
+    xinc1 = -1;
+    xinc2 = -1;
+  }
+
+  if (y1 >= y0)                 // The y-values are increasing
+  {
+    yinc1 = 1;
+    yinc2 = 1;
+  }
+  else                          // The y-values are decreasing
+  {
+    yinc1 = -1;
+    yinc2 = -1;
+  }
+
+  if (deltax >= deltay)         // There is at least one x-value for every y-value
+  {
+    xinc1 = 0;                  // Don't change the x when numerator >= denominator
+    yinc2 = 0;                  // Don't change the y for every iteration
+    den = deltax;
+    num = deltax / 2;
+    numadd = deltay;
+    numpixels = deltax;         // There are more x-values than y-values
+  }
+  else                          // There is at least one y-value for every x-value
+  {
+    xinc2 = 0;                  // Don't change the x for every iteration
+    yinc1 = 0;                  // Don't change the y when numerator >= denominator
+    den = deltay;
+    num = deltay / 2;
+    numadd = deltax;
+    numpixels = deltay;         // There are more y-values than x-values
+  }
+
+  for (int curpixel = 0; curpixel <= numpixels; curpixel++)
+  {
+    pt.x = x;      //Draw the current pixel
+    pt.y = y;
+    pts.push_back(pt);
+
+    num += numadd;              // Increase the numerator by the top of the fraction
+    if (num >= den)             // Check if numerator >= denominator
+    {
+      num -= den;               // Calculate the new numerator value
+      x += xinc1;               // Change the x as appropriate
+      y += yinc1;               // Change the y as appropriate
+    }
+    x += xinc2;                 // Change the x as appropriate
+    y += yinc2;                 // Change the y as appropriate
+  }
+}
+
+
+void FootprintHelper::getFillCells(std::vector<Cell>& footprint){
+  //quick bubble sort to sort pts by x
+  Cell swap, pt;
+  unsigned int i = 0;
+  while (i < footprint.size() - 1) {
+    if (footprint[i].x > footprint[i + 1].x) {
+      swap = footprint[i];
+      footprint[i] = footprint[i + 1];
+      footprint[i + 1] = swap;
+      if(i > 0) {
+        --i;
+      }
+    } else {
+      ++i;
+    }
+  }
+
+  i = 0;
+  Cell min_pt;
+  Cell max_pt;
+  unsigned int min_x = footprint[0].x;
+  unsigned int max_x = footprint[footprint.size() -1].x;
+  //walk through each column and mark cells inside the footprint
+  for (unsigned int x = min_x; x <= max_x; ++x) {
+    if (i >= footprint.size() - 1) {
+      break;
+    }
+    if (footprint[i].y < footprint[i + 1].y) {
+      min_pt = footprint[i];
+      max_pt = footprint[i + 1];
+    } else {
+      min_pt = footprint[i + 1];
+      max_pt = footprint[i];
+    }
+
+    i += 2;
+    while (i < footprint.size() && footprint[i].x == x) {
+      if(footprint[i].y < min_pt.y) {
+        min_pt = footprint[i];
+      } else if(footprint[i].y > max_pt.y) {
+        max_pt = footprint[i];
+      }
+      ++i;
+    }
+
+    //loop though cells in the column
+    for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
+      pt.x = x;
+      pt.y = y;
+      footprint.push_back(pt);
+    }
+  }
+}
+
+/**
+ * get the cells of a footprint at a given position
+ */
+std::vector<Cell> FootprintHelper::getFootprintCells(
+    double x, double y, double theta,
+    std::vector<geometry_msgs::Point> footprint_spec,
+    const costmap_2d::Costmap2D& costmap,
+    bool fill){
+  std::vector<Cell> footprint_cells;
+
+  //if we have no footprint... do nothing
+  if (footprint_spec.size() <= 1) {
+    unsigned int mx, my;
+    if (costmap.worldToMap(x, y, mx, my)) {
+      Cell center;
+      center.x = mx;
+      center.y = my;
+      footprint_cells.push_back(center);
+    }
+    return footprint_cells;
+  }
+
+  //pre-compute cos and sin values
+  double cos_th = cos(theta);
+  double sin_th = sin(theta);
+  double new_x, new_y;
+  unsigned int x0, y0, x1, y1;
+  unsigned int last_index = footprint_spec.size() - 1;
+
+  for (unsigned int i = 0; i < last_index; ++i) {
+    //find the cell coordinates of the first segment point
+    new_x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
+    new_y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
+    if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
+      return footprint_cells;
+    }
+
+    //find the cell coordinates of the second segment point
+    new_x = x + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
+    new_y = y + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
+    if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
+      return footprint_cells;
+    }
+
+    getLineCells(x0, x1, y0, y1, footprint_cells);
+  }
+
+  //we need to close the loop, so we also have to raytrace from the last pt to first pt
+  new_x = x + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
+  new_y = y + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
+  if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
+    return footprint_cells;
+  }
+  new_x = x + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
+  new_y = y + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
+  if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
+    return footprint_cells;
+  }
+
+  getLineCells(x0, x1, y0, y1, footprint_cells);
+
+  if(fill) {
+    getFillCells(footprint_cells);
+  }
+
+  return footprint_cells;
+}
+
+} /* namespace mbf_costmap_nav */

navigations/move_base_flex/mbf_costmap_nav/src/move_base_server_node.cpp

@@ -0,0 +1,83 @@
+/*
+ *  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_server_node.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "mbf_costmap_nav/costmap_navigation_server.h"
+#include <signal.h>
+#include <mbf_utility/types.h>
+#include <tf2_ros/transform_listener.h>
+
+typedef boost::shared_ptr<mbf_costmap_nav::CostmapNavigationServer> CostmapNavigationServerPtr;
+mbf_costmap_nav::CostmapNavigationServer::Ptr costmap_nav_srv_ptr;
+
+void sigintHandler(int sig)
+{
+  ROS_INFO_STREAM("Shutdown costmap navigation server.");
+  if(costmap_nav_srv_ptr)
+  {
+    costmap_nav_srv_ptr->stop();
+  }
+  ros::shutdown();
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mbf_2d_nav_server", ros::init_options::NoSigintHandler);
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  double cache_time;
+  private_nh.param("tf_cache_time", cache_time, 10.0);
+
+  signal(SIGINT, sigintHandler);
+#ifdef USE_OLD_TF
+  TFPtr tf_listener_ptr(new TF(nh, ros::Duration(cache_time), true));
+#else
+  TFPtr tf_listener_ptr(new TF(ros::Duration(cache_time)));
+  tf2_ros::TransformListener tf_listener(*tf_listener_ptr);
+#endif
+  costmap_nav_srv_ptr = boost::make_shared<mbf_costmap_nav::CostmapNavigationServer>(tf_listener_ptr);
+  ros::spin();
+
+  // explicitly call destructor here, otherwise costmap_nav_srv_ptr will be
+  // destructed after tearing down internally allocated static variables
+  costmap_nav_srv_ptr.reset();
+  return EXIT_SUCCESS;
+}

navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_global_planner.cpp

@@ -0,0 +1,82 @@
+/*
+ *  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.
+ *
+ *  wrapper_global_planner.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "nav_core_wrapper/wrapper_global_planner.h"
+
+namespace mbf_nav_core_wrapper
+{
+
+uint32_t WrapperGlobalPlanner::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)
+{
+#if ROS_VERSION_MINIMUM(1, 12, 0) // if current ros version is >= 1.12.0
+  // Kinetic and beyond
+  bool success = nav_core_plugin_->makePlan(start, goal, plan, cost);
+#else
+  // Indigo
+  bool success = nav_core_plugin_->makePlan(start, goal, plan);
+  cost = 0;
+#endif
+  message = success ? "Plan found" : "Planner failed";
+  return success ? 0 : 50;  // SUCCESS | FAILURE
+}
+
+bool WrapperGlobalPlanner::cancel()
+{
+  return false;
+}
+
+void WrapperGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
+{
+  nav_core_plugin_->initialize(name, costmap_ros);
+}
+
+WrapperGlobalPlanner::WrapperGlobalPlanner(boost::shared_ptr<nav_core::BaseGlobalPlanner> plugin)
+    : nav_core_plugin_(plugin)
+{}
+
+WrapperGlobalPlanner::~WrapperGlobalPlanner()
+{}
+
+};  /* namespace mbf_abstract_core */

navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp

@@ -0,0 +1,94 @@
+/*
+ *  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.
+ *
+ *  wrapper_local_planner.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "nav_core_wrapper/wrapper_local_planner.h"
+
+namespace mbf_nav_core_wrapper
+{
+
+uint32_t WrapperLocalPlanner::computeVelocityCommands(
+    const geometry_msgs::PoseStamped &robot_pose,
+    const geometry_msgs::TwistStamped &robot_velocity,
+    geometry_msgs::TwistStamped &cmd_vel,
+    std::string &message)
+{
+  bool success = nav_core_plugin_->computeVelocityCommands(cmd_vel.twist);
+  message = success ? "Goal reached" : "Controller failed";
+  return success ? 0 : 100;  // SUCCESS | FAILURE
+}
+
+bool WrapperLocalPlanner::isGoalReached()
+{
+  return nav_core_plugin_->isGoalReached();
+}
+
+bool WrapperLocalPlanner::isGoalReached(double xy_tolerance, double yaw_tolerance)
+{
+  return isGoalReached();
+}
+
+bool WrapperLocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &plan)
+{
+  return nav_core_plugin_->setPlan(plan);
+}
+
+bool WrapperLocalPlanner::cancel()
+{
+  ROS_WARN_STREAM("The cancel method is not implemented. "
+                  "Note: you are running a nav_core based plugin, "
+                  "which is wrapped into the MBF interface.");
+  return false;
+}
+
+void WrapperLocalPlanner::initialize(std::string name,
+                                     TF *tf,
+                                     costmap_2d::Costmap2DROS *costmap_ros)
+{
+  nav_core_plugin_->initialize(name, tf, costmap_ros);
+}
+
+WrapperLocalPlanner::WrapperLocalPlanner(boost::shared_ptr<nav_core::BaseLocalPlanner> plugin)
+    : nav_core_plugin_(plugin)
+{}
+
+WrapperLocalPlanner::~WrapperLocalPlanner()
+{}
+
+};  /* namespace mbf_abstract_core */

navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_recovery_behavior.cpp

@@ -0,0 +1,72 @@
+/*
+ *  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.
+ *
+ *  wrapper_recovery_behavior.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include <mbf_msgs/RecoveryResult.h>
+#include "nav_core_wrapper/wrapper_recovery_behavior.h"
+
+namespace mbf_nav_core_wrapper
+{
+void WrapperRecoveryBehavior::initialize(std::string name, TF *tf,
+                                         costmap_2d::Costmap2DROS *global_costmap,
+                                         costmap_2d::Costmap2DROS *local_costmap)
+{
+  nav_core_plugin_->initialize(name, tf, global_costmap, local_costmap);
+}
+
+uint32_t WrapperRecoveryBehavior::runBehavior(std::string &message)
+{
+  nav_core_plugin_->runBehavior();
+  // TODO return a code for old API
+  return mbf_msgs::RecoveryResult::SUCCESS;
+}
+
+bool WrapperRecoveryBehavior::cancel()
+{
+  return false;
+}
+
+WrapperRecoveryBehavior::WrapperRecoveryBehavior(boost::shared_ptr<nav_core::RecoveryBehavior> plugin)
+    : nav_core_plugin_(plugin)
+{}
+
+WrapperRecoveryBehavior::~WrapperRecoveryBehavior()
+{}
+
+};  /* namespace mbf_abstract_core */

navigations/move_base_flex/mbf_msgs/CHANGELOG.rst

@@ -0,0 +1,41 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mbf_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2020-05-25)
+------------------
+* add impassable outcome code for recovery behaviors
+* enable different goal tolerance values for each action 
+
+0.3.1 (2020-04-07)
+------------------
+
+0.3.0 (2020-03-31)
+------------------
+* add some more error codes, e.g. out of map, or map error
+* unify license declaration to BSD-3
+
+0.2.5 (2019-10-11)
+------------------
+
+0.2.4 (2019-06-16)
+------------------
+* Add check_point_cost service
+* Change current_twist for last_cmd_vel on exe_path/feedback
+
+0.2.3 (2018-11-14)
+------------------
+* Add outcome and message to the action's feedback in ExePath and MoveBase
+
+0.2.1 (2018-10-03)
+------------------
+
+0.2.0 (2018-09-11)
+------------------
+* Concurrency for planners, controllers and recovery behaviors
+* Adds concurrency slots to actions
+
+0.1.0 (2018-03-22)
+------------------
+* First release of move_base_flex for kinetic and lunar
+ca

navigations/move_base_flex/mbf_msgs/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mbf_msgs)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+  actionlib_msgs
+  genmsg
+  geometry_msgs
+  message_generation
+  message_runtime
+  nav_msgs
+  std_msgs
+)
+
+add_service_files(
+  DIRECTORY
+  srv
+  FILES
+  CheckPoint.srv
+  CheckPose.srv
+  CheckPath.srv
+)
+
+add_action_files(
+  DIRECTORY
+  action
+  FILES
+  GetPath.action
+  ExePath.action
+  Recovery.action
+  MoveBase.action
+)
+
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+  geometry_msgs
+  nav_msgs
+  std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+  actionlib_msgs
+  geometry_msgs
+  message_runtime
+  nav_msgs
+  std_msgs
+)

navigations/move_base_flex/mbf_msgs/README.md

@@ -0,0 +1,3 @@
+# Move Base Flex Messages, Services and Actions {#mainpage}
+
+This move_base_flex messages package provides the action definition files for the actions GetPath, ExePath, Recovery and MoveBase. The action servers providing these actions are implemented in [mbf_abstract_nav](wiki.ros.org/mbf_abstract_nav).

navigations/move_base_flex/mbf_msgs/action/ExePath.action

@@ -0,0 +1,60 @@
+# Follow the given path until completion or failure
+
+nav_msgs/Path path
+
+# Controller to use; defaults to the first one specified on "controllers" parameter
+string controller
+
+# use different slots for concurrency
+uint8 concurrency_slot
+
+# Optionally overrule goal tolerances
+bool tolerance_from_action
+float32 dist_tolerance
+float32 angle_tolerance
+
+---
+
+# Predefined success codes:
+uint8 SUCCESS         = 0
+# 1..9 are reserved as plugin specific non-error results
+
+# Predefined error codes:
+uint8 FAILURE         = 100  # Unspecified failure, only used for old, non-mfb_core based plugins
+uint8 CANCELED        = 101
+uint8 NO_VALID_CMD    = 102
+uint8 PAT_EXCEEDED    = 103
+uint8 COLLISION       = 104
+uint8 OSCILLATION     = 105
+uint8 ROBOT_STUCK     = 106
+uint8 MISSED_GOAL     = 107
+uint8 MISSED_PATH     = 108
+uint8 BLOCKED_PATH    = 109
+uint8 INVALID_PATH    = 110
+uint8 TF_ERROR        = 111
+uint8 NOT_INITIALIZED = 112
+uint8 INVALID_PLUGIN  = 113
+uint8 INTERNAL_ERROR  = 114
+uint8 OUT_OF_MAP      = 115  # The start and / or the goal are outside the map
+uint8 MAP_ERROR       = 116  # The map is not running properly
+uint8 STOPPED         = 117  # The controller execution has been stopped rigorously.
+
+# 121..149 are reserved as plugin specific errors
+
+uint32 outcome
+string message
+
+geometry_msgs/PoseStamped  final_pose
+float32 dist_to_goal
+float32 angle_to_goal
+
+---
+
+# Outcome of most recent controller cycle. Same values as in result
+uint32 outcome
+string message
+
+float32 dist_to_goal
+float32 angle_to_goal
+geometry_msgs/PoseStamped  current_pose
+geometry_msgs/TwistStamped last_cmd_vel  # last command calculated by the controller

navigations/move_base_flex/mbf_msgs/action/GetPath.action

@@ -0,0 +1,55 @@
+# Get a path from start_pose or current position to the target pose
+
+# Use start_pose or current position as the beginning of the path
+bool use_start_pose
+
+# The start pose for the path; optional, used if use_start_pose is true
+geometry_msgs/PoseStamped start_pose
+
+# The pose to achieve with the path
+geometry_msgs/PoseStamped target_pose
+
+# Planner to use; defaults to the first one specified on "planners" parameter
+string planner
+
+# use different slots for concurrency
+uint8 concurrency_slot
+
+# Optional: how many meters/radians away from the goal can end the created path. The final pose of the
+# path must be within tolerance range (position and orientation) for MBF to return a success outcome.
+bool tolerance_from_action
+float32 dist_tolerance
+float32 angle_tolerance
+
+---
+
+# Predefined success codes:
+uint8 SUCCESS         = 0
+# 1..9 are reserved as plugin specific non-error results
+
+# Possible error codes:
+uint8 FAILURE         = 50  # Unspecified failure, only used for old, non-mfb_core based plugins
+uint8 CANCELED        = 51  # The action has been canceled by a action client
+uint8 INVALID_START   = 52  #
+uint8 INVALID_GOAL    = 53
+uint8 NO_PATH_FOUND   = 54
+uint8 PAT_EXCEEDED    = 55
+uint8 EMPTY_PATH      = 56
+uint8 TF_ERROR        = 57
+uint8 NOT_INITIALIZED = 58
+uint8 INVALID_PLUGIN  = 59
+uint8 INTERNAL_ERROR  = 60
+uint8 OUT_OF_MAP      = 61
+uint8 MAP_ERROR       = 62
+uint8 STOPPED         = 63  # The planner execution has been stopped rigorously.
+
+# 71..99 are reserved as plugin specific errors
+
+uint32 outcome
+string message
+
+nav_msgs/Path path
+
+float64 cost
+
+---

navigations/move_base_flex/mbf_msgs/action/MoveBase.action

@@ -0,0 +1,60 @@
+# Extension of move_base_msgs/MoveBase action, with more detailed result
+# and feedback and the possibility to specify lists of applicable plugins
+
+geometry_msgs/PoseStamped target_pose
+
+# Controller to use; defaults to the first one specified on "controllers" parameter
+string controller
+
+# Planner to use; defaults to the first one specified on "planners" parameter
+string planner
+
+# Recovery behaviors to try on case of failure; defaults to the "recovery_behaviors" parameter value
+string[] recovery_behaviors
+
+# Optionally overrule controller's goal tolerances
+bool tolerance_from_action
+float32 dist_tolerance
+float32 angle_tolerance
+
+---
+
+# Predefined success codes:
+uint8 SUCCESS        = 0
+
+# Predefined general error codes:
+uint8 FAILURE        = 10
+uint8 CANCELED       = 11
+uint8 COLLISION      = 12
+uint8 OSCILLATION    = 13
+uint8 START_BLOCKED  = 14
+uint8 GOAL_BLOCKED   = 15
+uint8 TF_ERROR       = 16
+uint8 INTERNAL_ERROR = 17
+# 21..49 are reserved for future general error codes
+
+# Planning/controlling failures:
+uint8 PLAN_FAILURE   = 50
+# 51..99 are reserved as planner specific errors
+
+uint8 CTRL_FAILURE   = 100
+# 101..149 are reserved as controller specific errors
+
+uint32 outcome
+string message
+
+# Configuration upon action completion
+float32 dist_to_goal
+float32 angle_to_goal
+geometry_msgs/PoseStamped final_pose
+
+---
+
+# Outcome of most recent controller cycle. Same values as in MoveBase or ExePath result.
+uint32 outcome
+string message
+
+float32 dist_to_goal
+float32 angle_to_goal
+geometry_msgs/PoseStamped current_pose
+geometry_msgs/TwistStamped last_cmd_vel  # last command calculated by the controller

navigations/move_base_flex/mbf_msgs/action/Recovery.action

@@ -0,0 +1,30 @@
+# Run one of the recovery behavior listed on recovery_behaviors parameter
+
+string behavior
+
+# use different slots for concurrency
+uint8 concurrency_slot
+
+---
+
+# Predefined success codes:
+uint8 SUCCESS         = 0
+
+# Possible server codes:
+uint8 FAILURE         = 150
+uint8 CANCELED        = 151
+uint8 PAT_EXCEEDED    = 152
+uint8 TF_ERROR        = 153
+uint8 NOT_INITIALIZED = 154
+uint8 INVALID_PLUGIN  = 155
+uint8 INTERNAL_ERROR  = 156
+uint8 STOPPED         = 157  # The recovery behaviour execution has been stopped rigorously.
+uint8 IMPASSABLE      = 158  # Further execution would lead to a collision
+
+# 171..199 are reserved as plugin specific errors
+
+uint32 outcome
+string message
+string used_plugin
+
+---

navigations/move_base_flex/mbf_msgs/package.xml

@@ -0,0 +1,29 @@
+<package>
+    <name>mbf_msgs</name>
+    <version>0.3.2</version>
+    <description>
+        The move_base_flex messages package providing the action definition files for the action GetPath, ExePath, Recovery and MoveBase. The action servers providing these action are implemented in <a href="http://wiki.ros.org/mbf_abstract_nav">mbf_abstract_nav</a>.
+    </description>
+    <author email="santos@magazino.eu">Jorge Santos</author>
+    <author email="spuetz@uni-osnabrueck.de">Sebastian Pütz</author>
+    <maintainer email="santos@magazino.eu">Jorge Santos</maintainer>
+    <maintainer email="spuetz@uos.de">Sebastian Pütz</maintainer>
+    <license>BSD-3</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>std_msgs</build_depend>
+    <build_depend>nav_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <build_depend>actionlib_msgs</build_depend>
+    <build_depend>genmsg</build_depend>
+    <build_depend>message_runtime</build_depend>
+    <build_depend>message_generation</build_depend>
+
+    <run_depend>std_msgs</run_depend>
+    <run_depend>nav_msgs</run_depend>
+    <run_depend>geometry_msgs</run_depend>
+    <run_depend>actionlib_msgs</run_depend>
+    <run_depend>message_runtime</run_depend>
+
+</package>

navigations/move_base_flex/mbf_msgs/srv/CheckPath.srv

@@ -0,0 +1,24 @@
+uint8                      LOCAL_COSTMAP  = 1
+uint8                      GLOBAL_COSTMAP = 2
+
+nav_msgs/Path              path              # the path to be checked after transforming to costmap frame
+float32                    safety_dist       # minimum distance allowed to the closest obstacle (footprint padding)
+float32                    lethal_cost_mult  # cost multiplier for cells marked as lethal obstacle (zero is ignored)
+float32                    inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored)
+float32                    unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored)
+uint8                      costmap           # costmap in which to check the pose
+uint8                      return_on         # abort check on finding a pose with this state or worse (zero is ignored)
+uint8                      skip_poses        # skip this number of poses between checks, to speedup processing
+bool                       path_cells_only   # check only cells directly traversed by the path, ignoring robot footprint
+                                             # (if true, safety_dist is ignored)
+---
+uint8                      FREE      =  0    # robot is completely in traversable space
+uint8                      INSCRIBED =  1    # robot is partially in inscribed space
+uint8                      LETHAL    =  2    # robot is partially in collision
+uint8                      UNKNOWN   =  3    # robot is partially in unknown space
+uint8                      OUTSIDE   =  4    # robot is completely outside the map
+
+uint32                     last_checked      # index of the first pose along the path with return_on state or worse
+uint8                      state             # path worst state (until last_checked): FREE, INFLATED, LETHAL, UNKNOWN...
+uint32                     cost              # cost of all cells traversed by path within footprint padded by safety_dist
+                                             # or just by the path if path_cells_only is true

navigations/move_base_flex/mbf_msgs/srv/CheckPoint.srv

@@ -0,0 +1,14 @@
+uint8                      LOCAL_COSTMAP  = 1
+uint8                      GLOBAL_COSTMAP = 2
+
+geometry_msgs/PointStamped point             # the point to be checked after transforming to costmap frame
+uint8                      costmap           # costmap in which to check the point
+---
+uint8                      FREE      =  0    # point is in traversable space
+uint8                      INSCRIBED =  1    # point is in inscribed space
+uint8                      LETHAL    =  2    # point is in collision
+uint8                      UNKNOWN   =  3    # point is in unknown space
+uint8                      OUTSIDE   =  4    # point is outside the map
+
+uint8                      state             # point state: FREE, INFLATED, LETHAL, UNKNOWN or OUTSIDE
+uint32                     cost              # cost of the cell at point

navigations/move_base_flex/mbf_msgs/srv/CheckPose.srv

@@ -0,0 +1,19 @@
+uint8                      LOCAL_COSTMAP  = 1
+uint8                      GLOBAL_COSTMAP = 2
+
+geometry_msgs/PoseStamped  pose              # the pose to be checked after transforming to costmap frame
+float32                    safety_dist       # minimum distance allowed to the closest obstacle
+float32                    lethal_cost_mult  # cost multiplier for cells marked as lethal obstacle (zero is ignored)
+float32                    inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored)
+float32                    unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored)
+uint8                      costmap           # costmap in which to check the pose
+bool                       current_pose      # check current robot pose instead (ignores pose field)
+---
+uint8                      FREE      =  0    # robot is completely in traversable space
+uint8                      INSCRIBED =  1    # robot is partially in inscribed space
+uint8                      LETHAL    =  2    # robot is partially in collision
+uint8                      UNKNOWN   =  3    # robot is partially in unknown space
+uint8                      OUTSIDE   =  4    # robot is completely outside the map
+
+uint8                      state             # pose state: FREE, INFLATED, LETHAL, UNKNOWN or OUTSIDE
+uint32                     cost              # total cost of all cells within footprint padded by safety_dist

navigations/move_base_flex/mbf_simple_nav/CHANGELOG.rst

@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mbf_simple_nav
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2020-05-25)
+------------------
+
+0.3.1 (2020-04-07)
+------------------
+
+0.3.0 (2020-03-31)
+------------------
+* unify license declaration to BSD-3
+
+0.2.5 (2019-10-11)
+------------------
+
+0.2.4 (2019-06-16)
+------------------
+
+0.2.3 (2018-11-14)
+------------------
+
+0.2.2 (2018-10-10)
+------------------
+
+0.2.1 (2018-10-03)
+------------------
+* 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
+
+0.1.0 (2018-03-22)
+------------------
+* First release of move_base_flex for kinetic and lunar

navigations/move_base_flex/mbf_simple_nav/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mbf_simple_nav)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+  actionlib
+  actionlib_msgs
+  dynamic_reconfigure
+  geometry_msgs
+  mbf_abstract_nav
+  mbf_msgs
+  mbf_abstract_core
+  nav_msgs
+  pluginlib
+  roscpp
+  std_msgs
+  std_srvs
+  tf
+  tf2
+  tf2_ros
+  )
+
+find_package(Boost COMPONENTS thread chrono REQUIRED)
+
+set(MBF_SIMPLE_SERVER_LIB mbf_simple_server)
+set(MBF_SIMPLE_SERVER_NODE mbf_simple_nav)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${MBF_SIMPLE_SERVER_LIB}
+  CATKIN_DEPENDS
+  actionlib
+  actionlib_msgs
+  dynamic_reconfigure
+  geometry_msgs
+  mbf_abstract_nav
+  mbf_msgs
+  mbf_abstract_core
+  nav_msgs
+  pluginlib
+  roscpp
+  std_msgs
+  std_srvs
+  tf
+  tf2
+  tf2_ros
+  DEPENDS Boost
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+add_library(${MBF_SIMPLE_SERVER_LIB}
+  src/simple_navigation_server.cpp
+)
+
+add_dependencies(${MBF_SIMPLE_SERVER_LIB} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${MBF_SIMPLE_SERVER_LIB}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+add_executable(${MBF_SIMPLE_SERVER_NODE} src/simple_server_node.cpp)
+add_dependencies(${MBF_SIMPLE_SERVER_NODE} ${MBF_SIMPLE_SERVER_LIB})
+target_link_libraries(${MBF_SIMPLE_SERVER_NODE}
+  ${MBF_SIMPLE_SERVER_LIB}
+  ${catkin_LIBRARIES})
+
+install(TARGETS
+  ${MBF_SIMPLE_SERVER_LIB} ${MBF_SIMPLE_SERVER_NODE}
+  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_simple_nav/README.md

@@ -0,0 +1,6 @@
+# Move Base Flex Simple Navigation Server {#mainpage}
+
+
+The mbf_simple_nav package contains a simple navigation server implementation of Move Base Flex (MBF). The simple navigation server is bound to no map representation. It provides actions for planning, controlling and recovering. MBF loads all defined 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. 
+
+It tries to load the defined plugins which implements the defined interfaces in [mbf_abstract_core](wiki.ros.org/mbf_abstract_core).

navigations/move_base_flex/mbf_simple_nav/include/mbf_simple_nav/simple_navigation_server.h

@@ -0,0 +1,142 @@
+/*
+ *  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.
+ *
+ *  simple_navigation_server.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#ifndef MBF_SIMPLE_NAV__SIMPLE_NAVIGATION_SERVER_H_
+#define MBF_SIMPLE_NAV__SIMPLE_NAVIGATION_SERVER_H_
+
+#include <mbf_abstract_nav/abstract_navigation_server.h>
+#include <pluginlib/class_loader.h>
+#include <mbf_utility/types.h>
+
+namespace mbf_simple_nav
+{
+/**
+ * @defgroup simple_server Simple Server
+ * @brief Classes belonging to the Simple Server level.
+ */
+
+/**
+ * @brief The SimpleNavigationServer provides a simple navigation server, which does not bind a map representation to
+ *        Move Base Flex. It combines the execution classes which use the mbf_abstract_core/AbstractController,
+ *        mbf_abstract_core/AbstractPlanner and the mbf_abstract_core/AbstractRecovery base classes
+ *        as plugin interfaces.
+ *
+ * @ingroup navigation_server simple_server
+ */
+class SimpleNavigationServer : public mbf_abstract_nav::AbstractNavigationServer
+{
+public:
+
+  /**
+   * @brief Constructor
+   * @param tf_listener_ptr Shared pointer to a common TransformListener
+   */
+  SimpleNavigationServer(const TFPtr &tf_listener_ptr);
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~SimpleNavigationServer();
+
+  /**
+   * @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);
+
+  /**
+   * @brief Empty init method. Nothing to initialize.
+   * @param name The name of the controller
+   * @param controller_ptr pointer to the controller object which corresponds to the name param
+   * @return true always
+   */
+  virtual bool initializeControllerPlugin(
+      const std::string& name,
+      const mbf_abstract_core::AbstractController::Ptr& controller_ptr
+  );
+
+  /**
+   * @brief Loads the plugin associated with the given planner_type parameter.
+   * @param planner_type The type of the planner plugin to load.
+   * @return true, if the local planner plugin was successfully loaded.
+   */
+  virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string& planner_type);
+
+  /**
+   * @brief Empty init method. Nothing to initialize.
+   * @param name The name of the planner
+   * @param planner_ptr pointer to the planner object which corresponds to the name param
+   * @return true always
+   */
+  virtual bool initializePlannerPlugin(
+      const std::string& name,
+      const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr
+  );
+
+  /**
+   * @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);
+
+  /**
+   * @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 always
+   */
+  virtual bool initializeRecoveryPlugin(
+      const std::string& name,
+      const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr
+  );
+
+ private:
+  pluginlib::ClassLoader<mbf_abstract_core::AbstractPlanner> planner_plugin_loader_;
+  pluginlib::ClassLoader<mbf_abstract_core::AbstractController> controller_plugin_loader_;
+  pluginlib::ClassLoader<mbf_abstract_core::AbstractRecovery> recovery_plugin_loader_;
+};
+
+} /* namespace mbf_simple_nav */
+
+#endif /* MBF_SIMPLE_NAV__SIMPLE_NAVIGATION_SERVER_H_ */

navigations/move_base_flex/mbf_simple_nav/package.xml

@@ -0,0 +1,36 @@
+<package format="2">
+    <name>mbf_simple_nav</name>
+    <version>0.3.2</version>
+    <description>
+        The mbf_simple_nav package contains a simple navigation server implementation of Move Base Flex (MBF). The simple navigation server is bound to no map representation. It provides actions for planning, controlling and recovering. MBF loads all defined 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. 
+
+        It tries to load the defined plugins which implements the defined interfaces in <a href="wiki.ros.org/mbf_abstract_core">mbf_abstract_core</a>.
+    </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>
+
+    <depend>tf</depend>
+    <depend>roscpp</depend>
+    <depend>pluginlib</depend>
+    <depend>actionlib</depend>
+    <depend>actionlib_msgs</depend>
+    <depend>dynamic_reconfigure</depend>
+    <depend>std_msgs</depend>
+    <depend>std_srvs</depend>
+    <depend>nav_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>mbf_abstract_nav</depend>
+    <depend>mbf_abstract_core</depend>
+    <depend>mbf_msgs</depend>
+
+    <depend>tf2</depend>
+    <depend>tf2_ros</depend>
+    <export>
+      <rosdoc config="rosdoc.yaml" />
+    </export>
+</package>

navigations/move_base_flex/mbf_simple_nav/rosdoc.yaml

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

navigations/move_base_flex/mbf_simple_nav/src/simple_navigation_server.cpp

@@ -0,0 +1,142 @@
+/*
+ *  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.
+ *
+ *  simple_navigation_server.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "mbf_simple_nav/simple_navigation_server.h"
+
+namespace mbf_simple_nav
+{
+
+SimpleNavigationServer::SimpleNavigationServer(const TFPtr &tf_listener_ptr) :
+    mbf_abstract_nav::AbstractNavigationServer(tf_listener_ptr),
+    planner_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractPlanner"),
+    controller_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractController"),
+    recovery_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractRecovery")
+{
+  // initialize all plugins
+  initializeServerComponents();
+
+  // start all action servers
+  startActionServers();
+}
+
+mbf_abstract_core::AbstractPlanner::Ptr SimpleNavigationServer::loadPlannerPlugin(const std::string& planner_type)
+{
+  mbf_abstract_core::AbstractPlanner::Ptr planner_ptr;
+  ROS_INFO("Load global planner plugin.");
+  try
+  {
+    planner_ptr = planner_plugin_loader_.createInstance(planner_type);
+  }
+  catch (const pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it is properly registered"
+                                           << " and that the containing library is built? Exception: " << ex.what());
+  }
+  ROS_INFO("Global planner plugin loaded.");
+
+  return planner_ptr;
+}
+
+bool SimpleNavigationServer::initializePlannerPlugin(
+    const std::string& name,
+    const mbf_abstract_core::AbstractPlanner::Ptr&  planner_ptr
+)
+{
+  return true;
+}
+
+
+mbf_abstract_core::AbstractController::Ptr SimpleNavigationServer::loadControllerPlugin(
+    const std::string& controller_type)
+{
+  mbf_abstract_core::AbstractController::Ptr controller_ptr;
+  ROS_DEBUG("Load controller plugin.");
+  try
+  {
+    controller_ptr = controller_plugin_loader_.createInstance(controller_type);
+    ROS_INFO_STREAM("MBF_core-based local planner plugin " << controller_type << " loaded");
+  }
+  catch (const pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL_STREAM("Failed to load the " << controller_type
+                                           << " local planner, are you sure it's properly registered"
+                                           << " and that the containing library is built? Exception: " << ex.what());
+  }
+  return controller_ptr;
+}
+
+bool SimpleNavigationServer::initializeControllerPlugin(
+    const std::string& name,
+    const mbf_abstract_core::AbstractController::Ptr& controller_ptr)
+{
+  return true;
+}
+
+mbf_abstract_core::AbstractRecovery::Ptr SimpleNavigationServer::loadRecoveryPlugin(
+    const std::string& recovery_type)
+{
+  mbf_abstract_core::AbstractRecovery::Ptr recovery_ptr;
+
+  try
+  {
+    recovery_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractRecovery>(
+        recovery_plugin_loader_.createInstance(recovery_type));
+  }
+  catch (pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered"
+                                           << " and that the containing library is built? Exception: " << ex.what());
+  }
+  return recovery_ptr;
+}
+
+bool SimpleNavigationServer::initializeRecoveryPlugin(
+    const std::string& name,
+    const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr)
+{
+  return true;
+}
+
+
+SimpleNavigationServer::~SimpleNavigationServer()
+{
+}
+
+} /* namespace mbf_simple_nav */

navigations/move_base_flex/mbf_simple_nav/src/simple_server_node.cpp

@@ -0,0 +1,69 @@
+/*
+ *  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.
+ *
+ *  simple_server_node.cpp
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+
+#include "mbf_simple_nav/simple_navigation_server.h"
+#include <mbf_utility/types.h>
+#include <tf2_ros/transform_listener.h>
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mbf_simple_server");
+
+  typedef boost::shared_ptr<mbf_simple_nav::SimpleNavigationServer> SimpleNavigationServerPtr;
+
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh("~");
+
+  double cache_time;
+  private_nh.param("tf_cache_time", cache_time, 10.0);
+
+#ifdef USE_OLD_TF
+  TFPtr tf_listener_ptr(new TF(nh, ros::Duration(cache_time), true));
+#else
+  TFPtr tf_listener_ptr(new TF(ros::Duration(cache_time)));
+  tf2_ros::TransformListener tf_listener(*tf_listener_ptr);
+#endif 
+  
+  SimpleNavigationServerPtr controller_ptr(
+      new mbf_simple_nav::SimpleNavigationServer(tf_listener_ptr));
+
+  ros::spin();
+  return EXIT_SUCCESS;
+}