HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_controller_execution.h Executable file
View File

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

268
navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h Executable file
View File

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

137
navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_planner_execution.h Executable file
View File

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

120
navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_recovery_execution.h Executable file
View File

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

120
navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/costmap_wrapper.h Executable file
View File

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

89
navigations/move_base_flex/mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h Executable file
View File

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

105
navigations/move_base_flex/mbf_costmap_nav/include/nav_core_wrapper/wrapper_global_planner.h Executable file
View File

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

127
navigations/move_base_flex/mbf_costmap_nav/include/nav_core_wrapper/wrapper_local_planner.h Executable file
View File

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

96
navigations/move_base_flex/mbf_costmap_nav/include/nav_core_wrapper/wrapper_recovery_behavior.h Executable file
View File

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