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

55
navigations/move_base_flex/mbf_costmap_nav/CHANGELOG.rst Executable file
View File

@@ -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

110
navigations/move_base_flex/mbf_costmap_nav/CMakeLists.txt Executable file
View File

@@ -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}
)

8
navigations/move_base_flex/mbf_costmap_nav/README.md Executable file
View File

@@ -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)

21
navigations/move_base_flex/mbf_costmap_nav/cfg/MoveBaseFlex.cfg Executable file
View File

@@ -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"))

BIN
navigations/move_base_flex/mbf_costmap_nav/doc/images/mbf_costmap_nav.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 257 KiB

BIN
navigations/move_base_flex/mbf_costmap_nav/doc/images/mbf_costmap_nav_s.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 111 KiB

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

41
navigations/move_base_flex/mbf_costmap_nav/package.xml Executable file
View File

@@ -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>

4
navigations/move_base_flex/mbf_costmap_nav/rosdoc.yaml Executable file
View File

@@ -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'

144
navigations/move_base_flex/mbf_costmap_nav/scripts/move_base_legacy_relay.py Executable file
View File

@@ -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()

103
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp Executable file
View File

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

718
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp Executable file
View File

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

88
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_planner_execution.cpp Executable file
View File

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

72
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_recovery_execution.cpp Executable file
View File

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

139
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp Executable file
View File

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

237
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp Executable file
View File

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

83
navigations/move_base_flex/mbf_costmap_nav/src/move_base_server_node.cpp Executable file
View File

@@ -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;
}

82
navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_global_planner.cpp Executable file
View File

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

94
navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp Executable file
View File

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

72
navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_recovery_behavior.cpp Executable file
View File

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