├── .clang-format ├── .github └── workflows │ ├── format_check.yml │ └── noetic.yml ├── .gitignore ├── LICENSE ├── README.md ├── doc └── images │ ├── mbf_gridmap_nav.png │ ├── move_base_flex.pdf │ ├── move_base_flex.png │ └── move_base_flex.svg ├── git-clang-format.py ├── mbf_abstract_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mbf_abstract_core │ │ ├── abstract_controller.h │ │ ├── abstract_planner.h │ │ └── abstract_recovery.h ├── package.xml └── rosdoc.yaml ├── mbf_abstract_nav ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg │ └── MoveBaseFlex.cfg ├── doc │ └── images │ │ ├── mbf_abstract_nav.png │ │ └── mbf_abstract_nav_s.png ├── include │ └── mbf_abstract_nav │ │ ├── abstract_action_base.hpp │ │ ├── abstract_controller_execution.h │ │ ├── abstract_execution_base.h │ │ ├── abstract_navigation_server.h │ │ ├── abstract_planner_execution.h │ │ ├── abstract_plugin_manager.h │ │ ├── abstract_recovery_execution.h │ │ ├── controller_action.h │ │ ├── impl │ │ └── abstract_plugin_manager.tcc │ │ ├── move_base_action.h │ │ ├── planner_action.h │ │ └── recovery_action.h ├── package.xml ├── rosdoc.yaml ├── setup.py ├── src │ ├── abstract_controller_execution.cpp │ ├── abstract_execution_base.cpp │ ├── abstract_navigation_server.cpp │ ├── abstract_planner_execution.cpp │ ├── abstract_recovery_execution.cpp │ ├── controller_action.cpp │ ├── mbf_abstract_nav │ │ └── __init__.py │ ├── move_base_action.cpp │ ├── planner_action.cpp │ └── recovery_action.cpp └── test │ ├── abstract_action_base.cpp │ ├── abstract_action_base.launch │ ├── abstract_controller_execution.cpp │ ├── abstract_controller_execution.launch │ ├── abstract_execution_base.cpp │ ├── abstract_planner_execution.cpp │ ├── abstract_planner_execution.launch │ ├── planner_action.cpp │ └── planner_action.launch ├── mbf_costmap_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mbf_costmap_core │ │ ├── costmap_controller.h │ │ ├── costmap_planner.h │ │ └── costmap_recovery.h └── package.xml ├── mbf_costmap_nav ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg │ └── MoveBaseFlex.cfg ├── doc │ └── images │ │ ├── mbf_costmap_nav.png │ │ └── mbf_costmap_nav_s.png ├── include │ ├── mbf_costmap_nav │ │ ├── costmap_controller_execution.h │ │ ├── costmap_navigation_server.h │ │ ├── costmap_planner_execution.h │ │ ├── costmap_recovery_execution.h │ │ ├── costmap_wrapper.h │ │ ├── footprint_helper.h │ │ ├── free_pose_search.h │ │ └── free_pose_search_viz.h │ └── nav_core_wrapper │ │ ├── wrapper_global_planner.h │ │ ├── wrapper_local_planner.h │ │ └── wrapper_recovery_behavior.h ├── package.xml ├── rosdoc.yaml ├── scripts │ └── move_base_legacy_relay.py ├── src │ ├── mbf_costmap_nav │ │ ├── costmap_controller_execution.cpp │ │ ├── costmap_navigation_server.cpp │ │ ├── costmap_planner_execution.cpp │ │ ├── costmap_recovery_execution.cpp │ │ ├── costmap_wrapper.cpp │ │ ├── footprint_helper.cpp │ │ ├── free_pose_search.cpp │ │ └── free_pose_search_viz.cpp │ ├── move_base_server_node.cpp │ └── nav_core_wrapper │ │ ├── wrapper_global_planner.cpp │ │ ├── wrapper_local_planner.cpp │ │ └── wrapper_recovery_behavior.cpp └── test │ ├── config.yaml │ ├── free_pose_search.test │ ├── free_pose_search_test.cpp │ └── record_bag_file.sh ├── mbf_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── action │ ├── ExePath.action │ ├── GetPath.action │ ├── MoveBase.action │ └── Recovery.action ├── package.xml └── srv │ ├── CheckPath.srv │ ├── CheckPoint.srv │ ├── CheckPose.srv │ └── FindValidPose.srv ├── mbf_simple_nav ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mbf_simple_nav │ │ └── simple_navigation_server.h ├── package.xml ├── rosdoc.yaml └── src │ ├── simple_navigation_server.cpp │ └── simple_server_node.cpp ├── mbf_utility ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mbf_utility │ │ ├── exe_path_exception.h │ │ ├── get_path_exception.h │ │ ├── navigation_utility.h │ │ ├── odometry_helper.h │ │ ├── recovery_exception.h │ │ ├── robot_information.h │ │ └── types.h ├── package.xml └── src │ ├── navigation_utility.cpp │ ├── odometry_helper.cpp │ └── robot_information.cpp ├── move_base_flex ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml └── run-style-check-diff.sh /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AlwaysBreakTemplateDeclarations: true 12 | AlwaysBreakBeforeMultilineStrings: false 13 | BreakBeforeBinaryOperators: false 14 | BreakBeforeTernaryOperators: false 15 | BreakConstructorInitializersBeforeComma: true 16 | BinPackParameters: true 17 | ColumnLimit: 120 18 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 19 | DerivePointerBinding: false 20 | PointerBindsToType: true 21 | ExperimentalAutoDetectBinPacking: false 22 | IndentCaseLabels: true 23 | MaxEmptyLinesToKeep: 1 24 | NamespaceIndentation: None 25 | ObjCSpaceBeforeProtocolList: true 26 | PenaltyBreakBeforeFirstCallParameter: 19 27 | PenaltyBreakComment: 60 28 | PenaltyBreakString: 1 29 | PenaltyBreakFirstLessLess: 1000 30 | PenaltyExcessCharacter: 1000 31 | PenaltyReturnTypeOnItsOwnLine: 90 32 | SpacesBeforeTrailingComments: 2 33 | Cpp11BracedListStyle: false 34 | Standard: Auto 35 | IndentWidth: 2 36 | TabWidth: 2 37 | UseTab: Never 38 | IndentFunctionDeclarationAfterType: false 39 | SpacesInParentheses: false 40 | SpacesInAngles: false 41 | SpaceInEmptyParentheses: false 42 | SpacesInCStyleCastParentheses: false 43 | SpaceAfterControlStatementKeyword: true 44 | SpaceBeforeAssignmentOperators: true 45 | ContinuationIndentWidth: 4 46 | SortIncludes: false 47 | SpaceAfterCStyleCast: false 48 | 49 | # Configure each individual brace in BraceWrapping 50 | BreakBeforeBraces: Custom 51 | 52 | # Control of individual brace wrapping cases 53 | BraceWrapping: { 54 | AfterClass: 'true' 55 | AfterControlStatement: 'true' 56 | AfterEnum : 'true' 57 | AfterFunction : 'true' 58 | AfterNamespace : 'true' 59 | AfterStruct : 'true' 60 | AfterUnion : 'true' 61 | BeforeCatch : 'true' 62 | BeforeElse : 'true' 63 | IndentBraces : 'false' 64 | } 65 | ... 66 | -------------------------------------------------------------------------------- /.github/workflows/format_check.yml: -------------------------------------------------------------------------------- 1 | name: Format Check CI 2 | 3 | on: pull_request 4 | 5 | jobs: 6 | clang_format_check: 7 | name: clang format check 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/checkout@v2 11 | - name: the check 12 | run: sh ./run-style-check-diff.sh 13 | -------------------------------------------------------------------------------- /.github/workflows/noetic.yml: -------------------------------------------------------------------------------- 1 | name: Noetic CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - {ROS_DISTRO: noetic, ROS_REPO: testing} 11 | - {ROS_DISTRO: noetic, ROS_REPO: main} 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v2 15 | with: 16 | submodules: recursive 17 | - uses: 'ros-industrial/industrial_ci@master' 18 | env: ${{matrix.env}} 19 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .pyc 2 | .idea 3 | .project 4 | .cproject 5 | .settings 6 | .pydevproject 7 | cmake-build-debug 8 | !/.gitignore 9 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /doc/images/mbf_gridmap_nav.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/move_base_flex/367269c3c2427dfee29426e5f9f136731fb3b049/doc/images/mbf_gridmap_nav.png -------------------------------------------------------------------------------- /doc/images/move_base_flex.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/move_base_flex/367269c3c2427dfee29426e5f9f136731fb3b049/doc/images/move_base_flex.pdf -------------------------------------------------------------------------------- /doc/images/move_base_flex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/move_base_flex/367269c3c2427dfee29426e5f9f136731fb3b049/doc/images/move_base_flex.png -------------------------------------------------------------------------------- /mbf_abstract_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_abstract_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2021-10-26) 6 | ------------------ 7 | 8 | 0.3.4 (2020-12-02) 9 | ------------------ 10 | 11 | 0.3.3 (2020-11-05) 12 | ------------------ 13 | 14 | 0.3.2 (2020-05-25) 15 | ------------------ 16 | 17 | 0.3.1 (2020-04-07) 18 | ------------------ 19 | 20 | 0.3.0 (2020-03-31) 21 | ------------------ 22 | * unify license declaration to BSD-3 23 | 24 | 0.2.5 (2019-10-11) 25 | ------------------ 26 | 27 | 0.2.4 (2019-06-16) 28 | ------------------ 29 | 30 | 0.2.3 (2018-11-14) 31 | ------------------ 32 | 33 | 0.2.2 (2018-10-10) 34 | ------------------ 35 | 36 | 0.2.1 (2018-10-03) 37 | ------------------ 38 | 39 | 0.2.0 (2018-09-11) 40 | ------------------ 41 | * Update copyright and 3-clause-BSD license 42 | 43 | 0.1.0 (2018-03-22) 44 | ------------------ 45 | * First release of move_base_flex for kinetic and lunar 46 | -------------------------------------------------------------------------------- /mbf_abstract_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mbf_abstract_core) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | std_msgs 7 | geometry_msgs 8 | 9 | ) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS 13 | include 14 | CATKIN_DEPENDS 15 | std_msgs 16 | geometry_msgs 17 | ) 18 | 19 | 20 | ## Install project namespaced headers 21 | install(DIRECTORY include/${PROJECT_NAME}/ 22 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 23 | FILES_MATCHING PATTERN "*.h" 24 | PATTERN ".svn" EXCLUDE) 25 | -------------------------------------------------------------------------------- /mbf_abstract_core/README.md: -------------------------------------------------------------------------------- 1 | # Move Base Flex Abstract Core {#mainpage} 2 | 3 | The [mbf_abstract_core](wiki.ros.org/mbf_abstract_core) package of Move Base Flex (MBF) contains the abstract interfaces for planning controlling and recovering. Derived packages of the [mbf_abstract_nav](wiki.ros.org/mbf_abstract_nav) should use derived interfaces of the [mbf_abstract_core](wiki.ros.org/mbf_abstract_core) which then, for example, contain a method for initialization of the plugin, e.g. by handing over a pointer to the map representation of choice. 4 | 5 | The plugins which are loaded by Move Base Flex (a derived package of the mbf_abstract_nav) have to implement derived interfaces of the interfaces which are defined in this package. The mbf_abstract_core::AbstractPlanner provides an interface for a planner plugin to plan global paths. The mbf_abstract_core::AbstractController provides an interface for a controller plugin to control and move a robot. The mbf_abstract_core::AbstractRecovery provides an interface for a recovery plugin to recover the robot in deadlocked situations. 6 | -------------------------------------------------------------------------------- /mbf_abstract_core/include/mbf_abstract_core/abstract_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_planner.h 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #ifndef MBF_ABSTRACT_CORE__ABSTRACT_PLANNER_H_ 40 | #define MBF_ABSTRACT_CORE__ABSTRACT_PLANNER_H_ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace mbf_abstract_core 49 | { 50 | 51 | class AbstractPlanner{ 52 | 53 | public: 54 | typedef boost::shared_ptr< ::mbf_abstract_core::AbstractPlanner > Ptr; 55 | 56 | /** 57 | * @brief Destructor 58 | */ 59 | virtual ~AbstractPlanner(){}; 60 | 61 | /** 62 | * @brief Given a goal pose in the world, compute a plan 63 | * @param start The start pose 64 | * @param goal The goal pose 65 | * @param tolerance If the goal is obstructed, how many meters the planner can relax the constraint 66 | * in x and y before failing 67 | * @param plan The plan... filled by the planner 68 | * @param cost The cost for the the plan 69 | * @param message Optional more detailed outcome as a string 70 | * @return Result code as described on GetPath action result: 71 | * SUCCESS = 0 72 | * 1..9 are reserved as plugin specific non-error results 73 | * FAILURE = 50 # Unspecified failure, only used for old, non-mfb_core based plugins 74 | * CANCELED = 51 75 | * INVALID_START = 52 76 | * INVALID_GOAL = 53 77 | * BLOCKED_START = 54 78 | * BLOCKED_GOAL = 55 79 | * NO_PATH_FOUND = 56 80 | * PAT_EXCEEDED = 57 81 | * EMPTY_PATH = 58 82 | * TF_ERROR = 59 83 | * NOT_INITIALIZED = 60 84 | * INVALID_PLUGIN = 61 85 | * INTERNAL_ERROR = 62 86 | * 71..99 are reserved as plugin specific errors 87 | */ 88 | virtual uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 89 | double tolerance, std::vector &plan, double &cost, 90 | std::string &message) = 0; 91 | 92 | /** 93 | * @brief Requests the planner to cancel, e.g. if it takes too much time. 94 | * @return True if a cancel has been successfully requested, false if not implemented. 95 | */ 96 | virtual bool cancel() = 0; 97 | 98 | protected: 99 | /** 100 | * @brief Constructor 101 | */ 102 | AbstractPlanner(){}; 103 | }; 104 | } /* namespace mbf_abstract_core */ 105 | 106 | #endif /* MBF_ABSTRACT_CORE__ABSTRACT_PLANNER_H_ */ 107 | -------------------------------------------------------------------------------- /mbf_abstract_core/include/mbf_abstract_core/abstract_recovery.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_recovery.h 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #ifndef MBF_ABSTRACT_CORE__ABSTRACT_RECOVERY_H_ 40 | #define MBF_ABSTRACT_CORE__ABSTRACT_RECOVERY_H_ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | namespace mbf_abstract_core { 47 | /** 48 | * @class AbstractRecovery 49 | * @brief Provides an interface for recovery behaviors used in navigation. 50 | * All recovery behaviors written as plugins for the navigation stack must adhere to this interface. 51 | */ 52 | class AbstractRecovery{ 53 | public: 54 | 55 | typedef boost::shared_ptr< ::mbf_abstract_core::AbstractRecovery > Ptr; 56 | 57 | /** 58 | * @brief Runs the AbstractRecovery 59 | * @param message The recovery behavior could set, the message should correspond to the return value 60 | * @return An outcome which will be hand over to the action result. 61 | */ 62 | virtual uint32_t runBehavior(std::string& message) = 0; 63 | 64 | /** 65 | * @brief Virtual destructor for the interface 66 | */ 67 | virtual ~AbstractRecovery(){} 68 | 69 | /** 70 | * @brief Requests the recovery behavior to cancel, e.g. if it takes too much time. 71 | * @return True if a cancel has been successfully requested, false if not implemented. 72 | */ 73 | virtual bool cancel() = 0; 74 | 75 | protected: 76 | /** 77 | * @brief Constructor 78 | */ 79 | AbstractRecovery(){}; 80 | }; 81 | }; /* namespace mbf_abstract_core */ 82 | 83 | #endif /* MBF_ABSTRACT_CORE__ABSTRACT_RECOVERY_H_ */ 84 | -------------------------------------------------------------------------------- /mbf_abstract_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mbf_abstract_core 5 | 0.4.0 6 | 7 | This package provides common interfaces for navigation specific robot actions. It contains the AbstractPlanner, AbstractController and AbstractRecovery plugin interfaces. This interfaces have to be implemented by the plugins to make the plugin available for Move Base Flex. The abstract classes provides a meaningful interface enabling the planners, controllers and recovery behaviors to return information, e.g. why something went wrong. Derivided interfaces can, for example, provide methods to initialize the planner, controller or recovery with map representations like costmap_2d, grid_map or other representations. 8 | 9 | http://wiki.ros.org/mbf_abstract_core 10 | Sebastian Pütz 11 | Jorge Santos 12 | Sebastian Pütz 13 | Jorge Santos 14 | BSD-3 15 | catkin 16 | geometry_msgs 17 | std_msgs 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mbf_abstract_core/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: Move Base Flex 3 | homepage: http://wiki.ros.org/move_base_flex 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.py *.md *.rst' 5 | -------------------------------------------------------------------------------- /mbf_abstract_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mbf_abstract_nav) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | actionlib 7 | actionlib_msgs 8 | dynamic_reconfigure 9 | geometry_msgs 10 | mbf_msgs 11 | mbf_abstract_core 12 | mbf_utility 13 | nav_msgs 14 | roscpp 15 | std_msgs 16 | std_srvs 17 | tf 18 | xmlrpcpp 19 | ) 20 | 21 | find_package(Boost COMPONENTS thread chrono REQUIRED) 22 | 23 | # dynamic reconfigure: we provide the abstract configuration common to all MBF-based navigation 24 | # frameworks in a python module, so it can easily be included in particular navigation flavours 25 | catkin_python_setup() 26 | 27 | generate_dynamic_reconfigure_options( 28 | cfg/MoveBaseFlex.cfg 29 | ) 30 | 31 | set(MBF_ABSTRACT_SERVER_LIB mbf_abstract_server) 32 | 33 | catkin_package( 34 | INCLUDE_DIRS include 35 | LIBRARIES ${MBF_ABSTRACT_SERVER_LIB} 36 | CATKIN_DEPENDS 37 | actionlib 38 | actionlib_msgs 39 | dynamic_reconfigure 40 | geometry_msgs 41 | mbf_msgs 42 | mbf_abstract_core 43 | mbf_utility 44 | nav_msgs 45 | roscpp 46 | std_msgs 47 | std_srvs 48 | tf 49 | xmlrpcpp 50 | DEPENDS Boost 51 | ) 52 | 53 | 54 | include_directories( 55 | include 56 | ${catkin_INCLUDE_DIRS} 57 | ${Boost_INCLUDE_DIRS} 58 | ) 59 | 60 | add_library(${MBF_ABSTRACT_SERVER_LIB} 61 | src/controller_action.cpp 62 | src/planner_action.cpp 63 | src/recovery_action.cpp 64 | src/move_base_action.cpp 65 | src/abstract_execution_base.cpp 66 | src/abstract_navigation_server.cpp 67 | src/abstract_planner_execution.cpp 68 | src/abstract_controller_execution.cpp 69 | src/abstract_recovery_execution.cpp 70 | ) 71 | 72 | add_dependencies(${MBF_ABSTRACT_SERVER_LIB} ${PROJECT_NAME}_gencfg) 73 | add_dependencies(${MBF_ABSTRACT_SERVER_LIB} ${catkin_EXPORTED_TARGETS}) 74 | target_link_libraries(${MBF_ABSTRACT_SERVER_LIB} 75 | ${catkin_LIBRARIES} 76 | ${Boost_LIBRARIES} 77 | ) 78 | 79 | install(TARGETS 80 | ${MBF_ABSTRACT_SERVER_LIB} 81 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 84 | ) 85 | 86 | install(DIRECTORY include/${PROJECT_NAME}/ 87 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 88 | ) 89 | 90 | if(CATKIN_ENABLE_TESTING) 91 | find_package(rostest REQUIRED) 92 | 93 | # gtests 94 | catkin_add_gtest(${MBF_ABSTRACT_SERVER_LIB}_gtest test/abstract_execution_base.cpp) 95 | target_link_libraries(${MBF_ABSTRACT_SERVER_LIB}_gtest ${MBF_ABSTRACT_SERVER_LIB}) 96 | 97 | # ros-tests 98 | add_rostest_gmock(abstract_action_base_test 99 | test/abstract_action_base.launch 100 | test/abstract_action_base.cpp) 101 | target_link_libraries(abstract_action_base_test ${MBF_ABSTRACT_SERVER_LIB}) 102 | 103 | add_rostest_gmock(abstract_controller_execution_test 104 | test/abstract_controller_execution.launch 105 | test/abstract_controller_execution.cpp) 106 | target_link_libraries(abstract_controller_execution_test ${MBF_ABSTRACT_SERVER_LIB}) 107 | 108 | add_rostest_gmock(abstract_planner_execution_test 109 | test/abstract_planner_execution.launch 110 | test/abstract_planner_execution.cpp) 111 | target_link_libraries(abstract_planner_execution_test ${MBF_ABSTRACT_SERVER_LIB}) 112 | 113 | add_rostest_gmock(planner_action_test 114 | test/planner_action.launch 115 | test/planner_action.cpp) 116 | target_link_libraries(planner_action_test ${MBF_ABSTRACT_SERVER_LIB}) 117 | endif() 118 | -------------------------------------------------------------------------------- /mbf_abstract_nav/README.md: -------------------------------------------------------------------------------- 1 | # Move Base Flex Abstract Navigation Server {#mainpage} 2 | 3 | The mbf_abstract_nav package contains the abstract navigation server implementation of Move Base Flex (MBF). The abstract navigation server is not bound to any map representation. It provides the Actions for planning, controlling and recovering. At the time of start MBF loads all defined plugins. Therefor, it loads all plugins which are defined in the lists *planners*, *controllers* and *recovery_behaviors*. Each list holds a pair of a *name* and a *type*. The *type* defines which kind of plugin to load. The *name* defines under which name the plugin should be callable by the actions. 4 | 5 | ![mbf_abstract_nav sketch](doc/images/mbf_abstract_nav_s.png) -------------------------------------------------------------------------------- /mbf_abstract_nav/cfg/MoveBaseFlex.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'mbf_abstract_nav' 4 | 5 | from mbf_abstract_nav import add_mbf_abstract_nav_params 6 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t 7 | 8 | gen = ParameterGenerator() 9 | 10 | add_mbf_abstract_nav_params(gen) 11 | 12 | gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False) 13 | exit(gen.generate(PACKAGE, "move_base_flex_node", "MoveBaseFlex")) 14 | -------------------------------------------------------------------------------- /mbf_abstract_nav/doc/images/mbf_abstract_nav.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/move_base_flex/367269c3c2427dfee29426e5f9f136731fb3b049/mbf_abstract_nav/doc/images/mbf_abstract_nav.png -------------------------------------------------------------------------------- /mbf_abstract_nav/doc/images/mbf_abstract_nav_s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/move_base_flex/367269c3c2427dfee29426e5f9f136731fb3b049/mbf_abstract_nav/doc/images/mbf_abstract_nav_s.png -------------------------------------------------------------------------------- /mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_execution_base.h 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #ifndef MBF_ABSTRACT_NAV__ABSTRACT_EXECUTION_BASE_H_ 40 | #define MBF_ABSTRACT_NAV__ABSTRACT_EXECUTION_BASE_H_ 41 | 42 | #include 43 | 44 | #include 45 | 46 | #include 47 | 48 | #include 49 | 50 | namespace mbf_abstract_nav 51 | { 52 | /** 53 | * @brief Base class for running concurrent navigation tasks. 54 | * 55 | * The class uses a dedicated thread to run potentially long-lasting jobs. 56 | * The user can use waitForStateUpdate to get notification about the progress 57 | * of the said job. 58 | */ 59 | class AbstractExecutionBase 60 | { 61 | public: 62 | AbstractExecutionBase(const std::string& name, const mbf_utility::RobotInformation& robot_info); 63 | 64 | virtual ~AbstractExecutionBase(); 65 | 66 | virtual bool start(); 67 | 68 | virtual void stop(); 69 | 70 | /** 71 | * @brief Cancel the plugin execution. 72 | * @return true, if the plugin tries / tried to cancel the computation. 73 | */ 74 | virtual bool cancel() 75 | { 76 | return false; 77 | }; 78 | 79 | void join(); 80 | 81 | boost::cv_status waitForStateUpdate(boost::chrono::microseconds const& duration); 82 | 83 | /** 84 | * @brief Gets the current plugin execution outcome 85 | */ 86 | uint32_t getOutcome() const; 87 | 88 | /** 89 | * @brief Gets the current plugin execution message 90 | */ 91 | const std::string& getMessage() const; 92 | 93 | /** 94 | * @brief Returns the name of the corresponding plugin 95 | */ 96 | const std::string& getName() const; 97 | 98 | /** 99 | * @brief Optional implementation-specific setup function, called right before execution. 100 | */ 101 | virtual void preRun(){}; 102 | 103 | /** 104 | * @brief Optional implementation-specific cleanup function, called right after execution. 105 | */ 106 | virtual void postRun(){}; 107 | 108 | /** 109 | * @brief Optional implementation-specific configuration function. 110 | */ 111 | virtual void reconfigure(MoveBaseFlexConfig& _cfg) 112 | { 113 | } 114 | 115 | protected: 116 | virtual void run(){}; 117 | 118 | //! condition variable to wake up control thread 119 | boost::condition_variable condition_; 120 | 121 | //! the controlling thread object 122 | boost::thread thread_; 123 | 124 | //! flag for canceling controlling 125 | bool cancel_; 126 | 127 | //! the last received plugin execution outcome 128 | uint32_t outcome_; 129 | 130 | //! the last received plugin execution message 131 | std::string message_; 132 | 133 | //! the plugin name; not the plugin type! 134 | std::string name_; 135 | 136 | //! Reference to the current robot state 137 | const mbf_utility::RobotInformation& robot_info_; 138 | }; 139 | 140 | } /* namespace mbf_abstract_nav */ 141 | 142 | #endif /* MBF_ABSTRACT_NAV__ABSTRACT_EXECUTION_BASE_H_ */ 143 | -------------------------------------------------------------------------------- /mbf_abstract_nav/include/mbf_abstract_nav/abstract_plugin_manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_plugin_manager.h 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #ifndef MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_H_ 40 | #define MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_H_ 41 | 42 | #include 43 | 44 | namespace mbf_abstract_nav 45 | { 46 | 47 | template 48 | class AbstractPluginManager 49 | { 50 | public: 51 | 52 | typedef boost::function loadPluginFunction; 53 | typedef boost::function initPluginFunction; 54 | 55 | AbstractPluginManager( 56 | const std::string ¶m_name, 57 | const loadPluginFunction &loadPlugin, 58 | const initPluginFunction &initPlugin 59 | ); 60 | 61 | bool loadPlugins(); 62 | 63 | bool hasPlugin(const std::string &name); 64 | 65 | std::string getType(const std::string &name); 66 | 67 | const std::vector& getLoadedNames(); 68 | 69 | typename PluginType::Ptr getPlugin(const std::string &name); 70 | 71 | void clearPlugins(); 72 | 73 | protected: 74 | std::map plugins_; 75 | std::map plugins_type_; 76 | std::vector names_; 77 | const std::string param_name_; 78 | const loadPluginFunction loadPlugin_; 79 | const initPluginFunction initPlugin_; 80 | }; 81 | 82 | } /* namespace mbf_abstract_nav */ 83 | 84 | #include "impl/abstract_plugin_manager.tcc" 85 | #endif /* MBF_ABSTRACT_NAV__ABSTRACT_PLUGIN_MANAGER_H_ */ 86 | -------------------------------------------------------------------------------- /mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * controller_action.h 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_ 42 | #define MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_ 43 | 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | #include "mbf_abstract_nav/abstract_action_base.hpp" 50 | #include "mbf_abstract_nav/abstract_controller_execution.h" 51 | 52 | namespace mbf_abstract_nav 53 | { 54 | 55 | class ControllerAction : public AbstractActionBase 56 | { 57 | public: 58 | typedef boost::shared_ptr Ptr; 59 | 60 | ControllerAction(const std::string& name, const mbf_utility::RobotInformation& robot_info); 61 | 62 | void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig& config, uint32_t level) override; 63 | 64 | /** 65 | * @brief Start controller action. 66 | * Override abstract action version to allow updating current plan without stopping execution. 67 | * @param goal_handle Reference to the goal handle received on action execution callback. 68 | * @param execution_ptr Pointer to the execution descriptor. 69 | */ 70 | void start(GoalHandle& goal_handle, typename AbstractControllerExecution::Ptr execution_ptr); 71 | 72 | void runImpl(GoalHandle& goal_handle, AbstractControllerExecution& execution); 73 | 74 | protected: 75 | void publishExePathFeedback(GoalHandle& goal_handle, uint32_t outcome, const std::string& message, 76 | const geometry_msgs::TwistStamped& current_twist); 77 | 78 | /** 79 | * @brief Utility method to fill the ExePath action result in a single line 80 | * @param outcome ExePath action outcome 81 | * @param message ExePath action message 82 | * @param result The action result to fill 83 | */ 84 | void fillExePathResult(uint32_t outcome, const std::string& message, mbf_msgs::ExePathResult& result); 85 | 86 | boost::mutex goal_mtx_; ///< lock goal handle for updating it while running 87 | geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose 88 | geometry_msgs::PoseStamped goal_pose_; ///< Current goal pose 89 | 90 | //! Publish the current goal pose (the last pose of the path we are following) 91 | ros::Publisher goal_pub_; 92 | 93 | //! timeout after an oscillation is detected 94 | ros::Duration oscillation_timeout_; 95 | 96 | //! minimal move distance to not detect an oscillation 97 | double oscillation_distance_; 98 | 99 | //! minimal rotation to not detect an oscillation 100 | double oscillation_angle_; 101 | }; 102 | } // namespace mbf_abstract_nav 103 | 104 | #endif // MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_ 105 | -------------------------------------------------------------------------------- /mbf_abstract_nav/include/mbf_abstract_nav/planner_action.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * planner_action.h 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_ABSTRACT_NAV__PLANNER_ACTION_H_ 42 | #define MBF_ABSTRACT_NAV__PLANNER_ACTION_H_ 43 | 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | #include "mbf_abstract_nav/abstract_action_base.hpp" 50 | #include "mbf_abstract_nav/abstract_planner_execution.h" 51 | 52 | namespace mbf_abstract_nav 53 | { 54 | 55 | class PlannerAction : public AbstractActionBase 56 | { 57 | public: 58 | typedef boost::shared_ptr Ptr; 59 | 60 | PlannerAction(const std::string& name, const mbf_utility::RobotInformation& robot_info); 61 | 62 | void runImpl(GoalHandle& goal_handle, AbstractPlannerExecution& execution); 63 | 64 | protected: 65 | /** 66 | * @brief Transforms a plan to the global frame (global_frame_) coord system. 67 | * @param plan Input plan to be transformed. 68 | * @param global_plan Output plan, which is then transformed to the global frame. 69 | * @return true, if the transformation succeeded, false otherwise 70 | */ 71 | bool transformPlanToGlobalFrame(const std::vector& plan, 72 | std::vector& global_plan); 73 | 74 | private: 75 | //! Publisher to publish the current goal pose, which is used for path planning 76 | ros::Publisher goal_pub_; 77 | 78 | //! Path sequence counter 79 | unsigned int path_seq_count_; 80 | }; 81 | 82 | } // namespace mbf_abstract_nav 83 | 84 | #endif /* MBF_ABSTRACT_NAV__PLANNER_ACTION_H_ */ 85 | -------------------------------------------------------------------------------- /mbf_abstract_nav/include/mbf_abstract_nav/recovery_action.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * recovery_action.h 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_ABSTRACT_NAV__RECOVERY_ACTION_H_ 42 | #define MBF_ABSTRACT_NAV__RECOVERY_ACTION_H_ 43 | 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | #include "mbf_abstract_nav/abstract_action_base.hpp" 51 | #include "mbf_abstract_nav/abstract_recovery_execution.h" 52 | 53 | namespace mbf_abstract_nav 54 | { 55 | 56 | class RecoveryAction : public AbstractActionBase 57 | { 58 | public: 59 | 60 | typedef boost::shared_ptr Ptr; 61 | 62 | RecoveryAction(const std::string &name, const mbf_utility::RobotInformation &robot_info); 63 | 64 | void runImpl(GoalHandle &goal_handle, AbstractRecoveryExecution &execution); 65 | 66 | }; 67 | 68 | } /* mbf_abstract_nav */ 69 | 70 | #endif /* MBF_ABSTRACT_NAV__RECOVERY_ACTION_H_ */ 71 | -------------------------------------------------------------------------------- /mbf_abstract_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mbf_abstract_nav 3 | 0.4.0 4 | 5 | The mbf_abstract_nav package contains the abstract navigation server implementation of Move Base Flex (MBF). The abstract navigation server is not bound to any map representation. It provides the actions for planning, controlling and recovering. MBF loads all defined plugins at the program start. Therefor, it loads all plugins which are defined in the lists *planners*, *controllers* and *recovery_behaviors*. Each list holds a pair of a *name* and a *type*. The *type* defines which kind of plugin to load. The *name* defines under which name the plugin should be callable by the actions. 6 | 7 | http://wiki.ros.org/move_base_flex 8 | Sebastian Pütz 9 | Sebastian Pütz 10 | Jorge Santos 11 | BSD-3 12 | 13 | catkin 14 | 15 | tf 16 | roscpp 17 | actionlib 18 | actionlib_msgs 19 | dynamic_reconfigure 20 | std_msgs 21 | std_srvs 22 | nav_msgs 23 | geometry_msgs 24 | mbf_abstract_core 25 | mbf_msgs 26 | mbf_utility 27 | xmlrpcpp 28 | 29 | tf 30 | roscpp 31 | actionlib 32 | actionlib_msgs 33 | dynamic_reconfigure 34 | std_msgs 35 | std_srvs 36 | nav_msgs 37 | geometry_msgs 38 | mbf_abstract_core 39 | mbf_msgs 40 | mbf_utility 41 | xmlrpcpp 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /mbf_abstract_nav/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: Move Base Flex 3 | homepage: http://wiki.ros.org/move_base_flex 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.py *.md' 5 | -------------------------------------------------------------------------------- /mbf_abstract_nav/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup( 6 | packages = ['mbf_abstract_nav'], 7 | package_dir = {'': 'src'}, 8 | ) 9 | 10 | setup(**d) 11 | -------------------------------------------------------------------------------- /mbf_abstract_nav/src/abstract_execution_base.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_execution_base.cpp 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #include "mbf_abstract_nav/abstract_execution_base.h" 40 | 41 | namespace mbf_abstract_nav 42 | { 43 | AbstractExecutionBase::AbstractExecutionBase(const std::string& name, const mbf_utility::RobotInformation& robot_info) 44 | : outcome_(255), cancel_(false), name_(name), robot_info_(robot_info) 45 | { 46 | } 47 | 48 | AbstractExecutionBase::~AbstractExecutionBase() 49 | { 50 | if (thread_.joinable()) 51 | { 52 | // if the user forgets to call stop(), we have to kill it 53 | stop(); 54 | thread_.join(); 55 | } 56 | } 57 | 58 | bool AbstractExecutionBase::start() 59 | { 60 | if (thread_.joinable()) 61 | { 62 | // if the user forgets to call stop(), we have to kill it 63 | stop(); 64 | thread_.join(); 65 | } 66 | 67 | thread_ = boost::thread(&AbstractExecutionBase::run, this); 68 | return true; 69 | } 70 | 71 | void AbstractExecutionBase::stop() 72 | { 73 | ROS_WARN_STREAM("Try to stop the plugin \"" << name_ << "\" rigorously by interrupting the thread!"); 74 | thread_.interrupt(); 75 | } 76 | 77 | void AbstractExecutionBase::join() 78 | { 79 | if (thread_.joinable()) 80 | thread_.join(); 81 | } 82 | 83 | boost::cv_status AbstractExecutionBase::waitForStateUpdate(boost::chrono::microseconds const& duration) 84 | { 85 | boost::mutex mutex; 86 | boost::unique_lock lock(mutex); 87 | return condition_.wait_for(lock, duration); 88 | } 89 | 90 | uint32_t AbstractExecutionBase::getOutcome() const 91 | { 92 | return outcome_; 93 | } 94 | 95 | const std::string& AbstractExecutionBase::getMessage() const 96 | { 97 | return message_; 98 | } 99 | 100 | const std::string& AbstractExecutionBase::getName() const 101 | { 102 | return name_; 103 | } 104 | 105 | } /* namespace mbf_abstract_nav */ 106 | -------------------------------------------------------------------------------- /mbf_abstract_nav/src/abstract_recovery_execution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_recovery_execution.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #include 42 | 43 | #include 44 | 45 | namespace mbf_abstract_nav 46 | { 47 | 48 | AbstractRecoveryExecution::AbstractRecoveryExecution( 49 | const std::string &name, 50 | const mbf_abstract_core::AbstractRecovery::Ptr &recovery_ptr, 51 | const mbf_utility::RobotInformation &robot_info, 52 | const MoveBaseFlexConfig &config) : 53 | AbstractExecutionBase(name, robot_info), 54 | behavior_(recovery_ptr), state_(INITIALIZED) 55 | { 56 | // dynamically reconfigurable parameters 57 | reconfigure(config); 58 | } 59 | 60 | AbstractRecoveryExecution::~AbstractRecoveryExecution() 61 | { 62 | } 63 | 64 | 65 | void AbstractRecoveryExecution::reconfigure(const MoveBaseFlexConfig &config) 66 | { 67 | boost::lock_guard guard(conf_mtx_); 68 | 69 | // Maximum time allowed to recovery behaviors. Intended as a safeward for the case a behavior hangs. 70 | // If it doesn't return within time, the navigator will cancel it and abort the corresponding action. 71 | patience_ = ros::Duration(config.recovery_patience); 72 | 73 | // Nothing else to do here, as recovery_enabled is loaded and used in the navigation server 74 | } 75 | 76 | 77 | void AbstractRecoveryExecution::setState(RecoveryState state) 78 | { 79 | boost::lock_guard guard(state_mtx_); 80 | state_ = state; 81 | } 82 | 83 | 84 | typename AbstractRecoveryExecution::RecoveryState AbstractRecoveryExecution::getState() 85 | { 86 | boost::lock_guard guard(state_mtx_); 87 | return state_; 88 | } 89 | 90 | bool AbstractRecoveryExecution::cancel() 91 | { 92 | cancel_ = true; 93 | // returns false if cancel is not implemented or rejected by the recovery behavior (will run until completion) 94 | if (!behavior_->cancel()) 95 | { 96 | ROS_WARN_STREAM("Cancel recovery behavior \"" << name_ << "\" failed or is not supported by the plugin. " 97 | << "Wait until the current recovery behavior finished!"); 98 | return false; 99 | } 100 | return true; 101 | } 102 | 103 | bool AbstractRecoveryExecution::isPatienceExceeded() 104 | { 105 | boost::lock_guard guard1(conf_mtx_); 106 | boost::lock_guard guard2(time_mtx_); 107 | ROS_DEBUG_STREAM("Patience: " << patience_ << ", start time: " << start_time_ << " now: " << ros::Time::now()); 108 | return !patience_.isZero() && (ros::Time::now() - start_time_ > patience_); 109 | } 110 | 111 | void AbstractRecoveryExecution::run() 112 | { 113 | cancel_ = false; // reset the canceled state 114 | 115 | time_mtx_.lock(); 116 | start_time_ = ros::Time::now(); 117 | time_mtx_.unlock(); 118 | setState(RECOVERING); 119 | try 120 | { 121 | outcome_ = behavior_->runBehavior(message_); 122 | if (cancel_) 123 | { 124 | setState(CANCELED); 125 | } 126 | else 127 | { 128 | setState(RECOVERY_DONE); 129 | } 130 | } 131 | catch (boost::thread_interrupted &ex) 132 | { 133 | ROS_WARN_STREAM("Recovery \"" << name_ << "\" interrupted!"); 134 | setState(STOPPED); 135 | } 136 | catch (...) 137 | { 138 | ROS_FATAL_STREAM("Unknown error occurred in recovery behavior \"" << name_ << "\": " << boost::current_exception_diagnostic_information()); 139 | setState(INTERNAL_ERROR); 140 | } 141 | condition_.notify_one(); 142 | } 143 | 144 | } /* namespace mbf_abstract_nav */ 145 | -------------------------------------------------------------------------------- /mbf_abstract_nav/src/mbf_abstract_nav/__init__.py: -------------------------------------------------------------------------------- 1 | # Generic set of parameters required by any MBF-based navigation framework 2 | # To use: 3 | # 4 | # from mbf_abstract_nav import add_mbf_abstract_nav_params 5 | # gen = ParameterGenerator() 6 | # add_mbf_abstract_nav_params(gen) 7 | # ... 8 | # WARN: parameters added here must be copied on the specific MBF implementation reconfigure callback, e.g. in: 9 | # https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp#L130 10 | # Also, you need to touch https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/cfg/MoveBaseFlex.cfg 11 | # when recompiling to ensure configuration code is regenerated with the new parameters 12 | 13 | # need this only for dataype declarations 14 | from dynamic_reconfigure.parameter_generator_catkin import str_t, double_t, int_t, bool_t 15 | 16 | 17 | def add_mbf_abstract_nav_params(gen): 18 | gen.add("planner_frequency", double_t, 0, 19 | "The rate in Hz at which to run the planning loop", 0, 0, 100) 20 | gen.add("planner_patience", double_t, 0, 21 | "How long the planner will wait in seconds in an attempt to find a valid plan before giving up", 5.0, 0, 100) 22 | gen.add("planner_max_retries", int_t, 0, 23 | "How many times we will recall the planner in an attempt to find a valid plan before giving up", -1, -1, 1000) 24 | 25 | gen.add("controller_frequency", double_t, 0, 26 | "The rate in Hz at which to run the control loop and send velocity commands to the base", 20, 0, 100) 27 | gen.add("controller_patience", double_t, 0, 28 | "How long the controller will wait in seconds without receiving a valid control before giving up", 5.0, 0, 100) 29 | gen.add("controller_max_retries", int_t, 0, 30 | "How many times we will recall the controller in an attempt to find a valid command before giving up", -1, -1, 1000) 31 | 32 | gen.add("recovery_enabled", bool_t, 0, 33 | "Whether or not to enable the move_base_flex recovery behaviors to attempt to clear out space", True) 34 | gen.add("recovery_patience", double_t, 0, 35 | "How much time we allow recovery behaviors to complete before canceling (or stopping if cancel fails) them", 15.0, 0, 100) 36 | 37 | gen.add("oscillation_timeout", double_t, 0, 38 | "How long in seconds to allow for oscillation before executing recovery behaviors", 0.0, 0, 60) 39 | gen.add("oscillation_distance", double_t, 0, 40 | "How far in meters the robot must move to be considered not to be oscillating", 0.5, 0, 10) 41 | gen.add("oscillation_angle", double_t, 0, 42 | "How far in radian the robot must rotate to be considered not to be oscillating", 3.14, 0, 6.28) 43 | -------------------------------------------------------------------------------- /mbf_abstract_nav/test/abstract_action_base.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // dummy message 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | using namespace mbf_abstract_nav; 12 | 13 | // mocked version of an execution 14 | struct MockedExecution : public AbstractExecutionBase { 15 | typedef boost::shared_ptr Ptr; 16 | 17 | MockedExecution(const mbf_utility::RobotInformation& ri) : AbstractExecutionBase("mocked_execution", ri) {} 18 | 19 | MOCK_METHOD0(cancel, bool()); 20 | 21 | protected: 22 | MOCK_METHOD0(run, void()); 23 | }; 24 | 25 | using testing::Test; 26 | 27 | // fixture with access to the AbstractActionBase's internals 28 | struct AbstractActionBaseFixture 29 | : public AbstractActionBase, 30 | public Test { 31 | // required members for the c'tor 32 | TF tf_; 33 | std::string test_name; 34 | mbf_utility::RobotInformation ri; 35 | 36 | AbstractActionBaseFixture() 37 | : test_name("action_base"), 38 | ri(tf_, "global_frame", "local_frame", ros::Duration()), 39 | AbstractActionBase(test_name, ri) 40 | { 41 | } 42 | 43 | void runImpl(GoalHandle &goal_handle, MockedExecution &execution) {} 44 | }; 45 | 46 | TEST_F(AbstractActionBaseFixture, thread_stop) 47 | { 48 | unsigned char slot = 1; 49 | concurrency_slots_[slot].execution.reset(new MockedExecution(AbstractActionBaseFixture::ri)); 50 | concurrency_slots_[slot].thread_ptr = 51 | threads_.create_thread(boost::bind(&AbstractActionBaseFixture::run, this, 52 | boost::ref(concurrency_slots_[slot]))); 53 | } 54 | 55 | using testing::Return; 56 | 57 | TEST_F(AbstractActionBaseFixture, cancelAll) 58 | { 59 | // spawn a bunch of threads 60 | for (unsigned char slot = 0; slot != 10; ++slot) { 61 | concurrency_slots_[slot].execution.reset(new MockedExecution(AbstractActionBaseFixture::ri)); 62 | // set the expectation 63 | EXPECT_CALL(*concurrency_slots_[slot].execution, cancel()) 64 | .WillRepeatedly(Return(true)); 65 | 66 | // set the in_use flag --> this should turn to false 67 | concurrency_slots_[slot].in_use = true; 68 | concurrency_slots_[slot].thread_ptr = threads_.create_thread( 69 | boost::bind(&AbstractActionBaseFixture::run, this, 70 | boost::ref(concurrency_slots_[slot]))); 71 | } 72 | 73 | // cancel all of slots 74 | cancelAll(); 75 | 76 | // check the result 77 | for (ConcurrencyMap::iterator slot = concurrency_slots_.begin(); 78 | slot != concurrency_slots_.end(); ++slot) 79 | ASSERT_FALSE(slot->second.in_use); 80 | } 81 | 82 | int main(int argc, char **argv) 83 | { 84 | // we need this only for kinetic and lunar distros 85 | ros::init(argc, argv, "abstract_action_base"); 86 | ros::NodeHandle nh; 87 | testing::InitGoogleTest(&argc, argv); 88 | return RUN_ALL_TESTS(); 89 | } -------------------------------------------------------------------------------- /mbf_abstract_nav/test/abstract_action_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /mbf_abstract_nav/test/abstract_controller_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /mbf_abstract_nav/test/abstract_execution_base.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | using namespace mbf_abstract_nav; 7 | 8 | // our dummy implementation of the AbstractExecutionBase 9 | // it basically runs until we cancel it. 10 | struct DummyExecutionBase : public AbstractExecutionBase 11 | { 12 | DummyExecutionBase(const std::string& _name, const mbf_utility::RobotInformation& ri) 13 | : AbstractExecutionBase(_name, ri) 14 | { 15 | } 16 | 17 | // implement the required interfaces 18 | bool cancel() 19 | { 20 | cancel_ = true; 21 | condition_.notify_all(); 22 | return true; 23 | } 24 | 25 | protected: 26 | void run() 27 | { 28 | boost::mutex mutex; 29 | boost::unique_lock lock(mutex); 30 | 31 | // wait until someone says we are done (== cancel or stop) 32 | // we set a timeout, since we might miss the cancel call (especially if we 33 | // run on an environment with high CPU load) 34 | condition_.wait_for(lock, boost::chrono::seconds(1)); 35 | outcome_ = 0; 36 | } 37 | }; 38 | 39 | // shortcuts... 40 | using testing::Test; 41 | 42 | // the fixture owning the instance of the DummyExecutionBase 43 | struct AbstractExecutionFixture : public Test 44 | { 45 | TF tf_; 46 | DummyExecutionBase impl_; 47 | mbf_utility::RobotInformation ri_; 48 | 49 | AbstractExecutionFixture() : ri_(tf_, "global_frame", "local_frame", ros::Duration(), ""), impl_("foo", ri_) 50 | { 51 | } 52 | }; 53 | 54 | TEST_F(AbstractExecutionFixture, timeout) 55 | { 56 | // start the thread 57 | impl_.start(); 58 | 59 | // make sure that we timeout and don't alter the outcome 60 | EXPECT_EQ(impl_.waitForStateUpdate(boost::chrono::microseconds(60)), boost::cv_status::timeout); 61 | EXPECT_EQ(impl_.getOutcome(), 255); 62 | } 63 | 64 | TEST_F(AbstractExecutionFixture, success) 65 | { 66 | // start the thread 67 | impl_.start(); 68 | EXPECT_EQ(impl_.waitForStateUpdate(boost::chrono::microseconds(60)), boost::cv_status::timeout); 69 | 70 | // cancel, so we set the outcome to 0 71 | impl_.cancel(); 72 | impl_.join(); 73 | EXPECT_EQ(impl_.getOutcome(), 0); 74 | } 75 | 76 | TEST_F(AbstractExecutionFixture, restart) 77 | { 78 | // call start multiple times without waiting for its termination 79 | for (size_t ii = 0; ii != 10; ++ii) 80 | impl_.start(); 81 | } 82 | 83 | TEST_F(AbstractExecutionFixture, stop) 84 | { 85 | // call stop/terminate multiple times. this should be a noop 86 | for (size_t ii = 0; ii != 10; ++ii) 87 | { 88 | impl_.stop(); 89 | impl_.join(); 90 | } 91 | } 92 | 93 | int main(int argc, char** argv) 94 | { 95 | testing::InitGoogleTest(&argc, argv); 96 | return RUN_ALL_TESTS(); 97 | } -------------------------------------------------------------------------------- /mbf_abstract_nav/test/abstract_planner_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /mbf_abstract_nav/test/planner_action.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /mbf_costmap_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_costmap_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2021-10-26) 6 | ------------------ 7 | 8 | 0.3.4 (2020-12-02) 9 | ------------------ 10 | 11 | 0.3.3 (2020-11-05) 12 | ------------------ 13 | * Fix reference to TF is ambiguous, see #221 14 | 15 | 0.3.2 (2020-05-25) 16 | ------------------ 17 | 18 | 0.3.1 (2020-04-07) 19 | ------------------ 20 | 21 | 0.3.0 (2020-03-31) 22 | ------------------ 23 | * unify license declaration to BSD-3 24 | 25 | 0.2.5 (2019-10-11) 26 | ------------------ 27 | 28 | 0.2.4 (2019-06-16) 29 | ------------------ 30 | 31 | 0.2.3 (2018-11-14) 32 | ------------------ 33 | 34 | 0.2.2 (2018-10-10) 35 | ------------------ 36 | 37 | 0.2.1 (2018-10-03) 38 | ------------------ 39 | * Make MBF melodic and indigo compatible 40 | * Fix GoalHandle references bug in callbacks 41 | 42 | 0.2.0 (2018-09-11) 43 | ------------------ 44 | * Concurrency for planners, controllers and recovery behaviors 45 | 46 | 0.1.0 (2018-03-22) 47 | ------------------ 48 | * First release of move_base_flex for kinetic and lunar 49 | -------------------------------------------------------------------------------- /mbf_costmap_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mbf_costmap_core) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | std_msgs 7 | geometry_msgs 8 | mbf_abstract_core 9 | mbf_utility 10 | tf 11 | costmap_2d 12 | nav_core 13 | ) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS 17 | include 18 | CATKIN_DEPENDS 19 | std_msgs 20 | geometry_msgs 21 | mbf_abstract_core 22 | mbf_utility 23 | tf 24 | costmap_2d 25 | nav_core 26 | ) 27 | 28 | ## Install project namespaced headers 29 | install(DIRECTORY include/${PROJECT_NAME}/ 30 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 31 | FILES_MATCHING PATTERN "*.h" 32 | PATTERN ".svn" EXCLUDE) 33 | -------------------------------------------------------------------------------- /mbf_costmap_core/README.md: -------------------------------------------------------------------------------- 1 | # Move Base Flex Costmap Navigation Core {#mainpage} 2 | 3 | This package provides common interfaces for navigation specific robot actions. It contains the CostmapPlanner, CostmapController and CostmapRecovery interfaces. The interfaces have to be implemented by the plugins to make them available for Move Base Flex using the [mbf_costmap_nav](wiki.ros.org/mbf_costmap_nav) navigation implementation. That implementation inherits the [mbf_abstract_nav](wiki.ros.org/mbf_abstract_nav) implementation and binds the system to a local and a global costmap. -------------------------------------------------------------------------------- /mbf_costmap_core/include/mbf_costmap_core/costmap_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_global_planner.h 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #ifndef MBF_COSTMAP_CORE__COSTMAP_PLANNER_H_ 40 | #define MBF_COSTMAP_CORE__COSTMAP_PLANNER_H_ 41 | 42 | #include 43 | #include 44 | 45 | namespace mbf_costmap_core { 46 | /** 47 | * @class CostmapPlanner 48 | * @brief Provides an interface for global planners used in navigation. 49 | * All global planners written to work as MBF plugins must adhere to this interface. Alternatively, this 50 | * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility. 51 | */ 52 | class CostmapPlanner : public mbf_abstract_core::AbstractPlanner{ 53 | public: 54 | 55 | typedef boost::shared_ptr< ::mbf_costmap_core::CostmapPlanner > Ptr; 56 | 57 | /** 58 | * @brief Given a goal pose in the world, compute a plan 59 | * @param start The start pose 60 | * @param goal The goal pose 61 | * @param tolerance If the goal is obstructed, how many meters the planner can relax the constraint 62 | * in x and y before failing 63 | * @param plan The plan... filled by the planner 64 | * @param cost The cost for the the plan 65 | * @param message Optional more detailed outcome as a string 66 | * @return Result code as described on GetPath action result: 67 | * SUCCESS = 0 68 | * 1..9 are reserved as plugin specific non-error results 69 | * FAILURE = 50 # Unspecified failure, only used for old, non-mfb_core based plugins 70 | * CANCELED = 51 71 | * INVALID_START = 52 72 | * INVALID_GOAL = 53 73 | * BLOCKED_START = 54 74 | * BLOCKED_GOAL = 55 75 | * NO_PATH_FOUND = 56 76 | * PAT_EXCEEDED = 57 77 | * EMPTY_PATH = 58 78 | * TF_ERROR = 59 79 | * NOT_INITIALIZED = 60 80 | * INVALID_PLUGIN = 61 81 | * INTERNAL_ERROR = 62 82 | * 71..99 are reserved as plugin specific errors 83 | */ 84 | virtual uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 85 | double tolerance, std::vector &plan, double &cost, 86 | std::string &message) = 0; 87 | 88 | /** 89 | * @brief Requests the planner to cancel, e.g. if it takes too much time. 90 | * @remark New on MBF API 91 | * @return True if a cancel has been successfully requested, false if not implemented. 92 | */ 93 | virtual bool cancel() = 0; 94 | 95 | /** 96 | * @brief Initialization function for the CostmapPlanner 97 | * @param name The name of this planner 98 | * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning 99 | */ 100 | virtual void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) = 0; 101 | 102 | /** 103 | * @brief Virtual destructor for the interface 104 | */ 105 | virtual ~CostmapPlanner(){} 106 | 107 | protected: 108 | CostmapPlanner(){} 109 | 110 | }; 111 | } /* namespace mbf_costmap_core */ 112 | 113 | #endif /* MBF_COSTMAP_CORE__COSTMAP_PLANNER_H_ */ 114 | -------------------------------------------------------------------------------- /mbf_costmap_core/include/mbf_costmap_core/costmap_recovery.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_global_planner.h 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #ifndef MBF_COSTMAP_CORE__COSTMAP_RECOVERY_H_ 40 | #define MBF_COSTMAP_CORE__COSTMAP_RECOVERY_H_ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | namespace mbf_costmap_core 47 | { 48 | /** 49 | * @class CostmapRecovery 50 | * @brief Provides an interface for recovery behaviors used in navigation. 51 | * All recovery behaviors written to work as MBF plugins must adhere to this interface. Alternatively, this 52 | * class can also operate as a wrapper for old API nav_corebased plugins, providing backward compatibility. 53 | */ 54 | class CostmapRecovery : public mbf_abstract_core::AbstractRecovery{ 55 | public: 56 | 57 | typedef boost::shared_ptr< ::mbf_costmap_core::CostmapRecovery> Ptr; 58 | 59 | /** 60 | * @brief Initialization function for the CostmapRecovery 61 | * @param tf A pointer to a transform listener 62 | * @param global_costmap A pointer to the global_costmap used by the navigation stack 63 | * @param local_costmap A pointer to the local_costmap used by the navigation stack 64 | */ 65 | virtual void initialize(std::string name, TF* tf, 66 | costmap_2d::Costmap2DROS* global_costmap, 67 | costmap_2d::Costmap2DROS* local_costmap) = 0; 68 | 69 | /** 70 | * @brief Runs the CostmapRecovery 71 | * @param message The recovery behavior could set, the message should correspond to the return value 72 | * @return An outcome which will be hand over to the action result. 73 | */ 74 | virtual uint32_t runBehavior(std::string& message) = 0; 75 | 76 | /** 77 | * @brief Requests the planner to cancel, e.g. if it takes too much time 78 | * @remark New on MBF API 79 | * @return True if a cancel has been successfully requested, false if not implemented. 80 | */ 81 | virtual bool cancel() = 0; 82 | 83 | /** 84 | * @brief Virtual destructor for the interface 85 | */ 86 | virtual ~CostmapRecovery(){} 87 | 88 | protected: 89 | CostmapRecovery(){} 90 | 91 | }; 92 | } /* namespace mbf_costmap_core */ 93 | 94 | #endif /* MBF_COSTMAP_CORE__COSTMAP_RECOVERY_H_ */ 95 | -------------------------------------------------------------------------------- /mbf_costmap_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mbf_costmap_core 5 | 0.4.0 6 | 7 | This package provides common interfaces for navigation specific robot actions. It contains the CostmapPlanner, CostmapController and CostmapRecovery interfaces. The interfaces have to be implemented by the plugins to make them available for Move Base Flex using the mbf_costmap_nav navigation implementation. That implementation inherits the mbf_abstract_nav implementation and binds the system to a local and a global costmap. 8 | 9 | http://wiki.ros.org/move_base_flex/mbf_costmap_core 10 | Sebastian Pütz 11 | Jorge Santos 12 | Jorge Santos 13 | Sebastian Pütz 14 | BSD-3 15 | 16 | catkin 17 | 18 | costmap_2d 19 | geometry_msgs 20 | mbf_abstract_core 21 | mbf_utility 22 | nav_core 23 | std_msgs 24 | tf 25 | 26 | 27 | -------------------------------------------------------------------------------- /mbf_costmap_nav/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_costmap_nav 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2021-10-26) 6 | ------------------ 7 | * make the costmaps used for planning and controlling configurable, see #278 8 | * return empty footprint if partly outside of the map, see #272 9 | * add tf_transform_ptr to the abstract planner execution, see #256 10 | * transform pose to global frame before calling planner in costmap planner execution, see #256 11 | 12 | 0.3.4 (2020-12-02) 13 | ------------------ 14 | * fix blind driving, see #243 15 | 16 | 0.3.3 (2020-11-05) 17 | ------------------ 18 | * Fix controller fails if robot pose gets older than tf_timeout, see #231 19 | * clear the costmap before deactivating it, see #220 20 | * Use `catkin_install_python` to install legacy relay. see #219 21 | * For move_base_legacy_relay, keep configured base local and global planners to send to MBF, see #209 22 | * Fix deallocation on shutdown by discarding all plugins and resetting action server pointers, see #199 23 | * Make reference symbol position consistent across the project, see #198 24 | * Move RobotInformation to mbf_utility, as it can be used generaly, see #196 25 | * Prevent unrelated type casts for Cell, see #197 26 | 27 | 0.3.2 (2020-05-25) 28 | ------------------ 29 | * Remove dependency on base_local_planner and move FootprintHelper class to mbf_costmap_nav and make it static 30 | 31 | 0.3.1 (2020-04-07) 32 | ------------------ 33 | * Ensure that check_costmap_mutex is destroyed after timer. 34 | * Avoid crash on shutdown by stop shutdown_costmap_timer on destructor 35 | and explicitly call the costmap_nav_srv destructor 36 | 37 | 0.3.0 (2020-03-31) 38 | ------------------ 39 | * add output for cancel method if nav_core plugin is wrapped 40 | * unify license declaration to BSD-3 41 | 42 | 0.2.5 (2019-10-11) 43 | ------------------ 44 | * Add clear_on_shutdown functionality 45 | * Do not pass boost functions to abstract server to (de)activate costmaps. 46 | Run instead abstract methods (possibly) overridden in the costmap server, 47 | all costmap-related handling refactored to a new CostmapWrapper class 48 | * On controller execution, check that local costmap is current 49 | 50 | 0.2.4 (2019-06-16) 51 | ------------------ 52 | * Add check_point_cost service 53 | * Lock costmaps on clear_costmaps service 54 | * Replace recursive mutexes with normal ones when not needed 55 | 56 | 0.2.3 (2018-11-14) 57 | ------------------ 58 | * single publisher for controller execution objects 59 | 60 | 0.2.2 (2018-10-10) 61 | ------------------ 62 | * Do not use MultiThreadedSpinner, as costmap updates can crash when combining laser scans and point clouds 63 | * Make start/stop costmaps mutexed, since concurrent calls to start can lead to segfaults 64 | 65 | 0.2.1 (2018-10-03) 66 | ------------------ 67 | * Make MBF melodic and indigo compatible 68 | * Fix GoalHandle references bug in callbacks 69 | 70 | 0.2.0 (2018-09-11) 71 | ------------------ 72 | * Update copyright and 3-clause-BSD license 73 | * Concurrency for planners, controllers and recovery behaviors 74 | 75 | 0.1.0 (2018-03-22) 76 | ------------------ 77 | * First release of move_base_flex for kinetic and lunar 78 | -------------------------------------------------------------------------------- /mbf_costmap_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mbf_costmap_nav) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED 7 | COMPONENTS 8 | angles 9 | costmap_2d 10 | dynamic_reconfigure 11 | geometry_msgs 12 | mbf_abstract_nav 13 | mbf_costmap_core 14 | mbf_msgs 15 | mbf_utility 16 | nav_core 17 | nav_msgs 18 | roscpp 19 | std_msgs 20 | std_srvs 21 | tf 22 | ) 23 | 24 | find_package(Boost COMPONENTS thread chrono REQUIRED) 25 | 26 | # dynamic reconfigure 27 | generate_dynamic_reconfigure_options( 28 | cfg/MoveBaseFlex.cfg 29 | ) 30 | 31 | set(MBF_NAV_CORE_WRAPPER_LIB mbf_nav_core_wrapper) 32 | set(MBF_COSTMAP_2D_SERVER_LIB mbf_costmap_server) 33 | set(MBF_COSTMAP_2D_SERVER_NODE mbf_costmap_nav) 34 | 35 | catkin_package( 36 | INCLUDE_DIRS include 37 | LIBRARIES ${MBF_COSTMAP_2D_SERVER_LIB} 38 | CATKIN_DEPENDS 39 | actionlib 40 | actionlib_msgs 41 | angles 42 | costmap_2d 43 | dynamic_reconfigure 44 | geometry_msgs 45 | mbf_abstract_nav 46 | mbf_costmap_core 47 | mbf_msgs 48 | mbf_utility 49 | nav_core 50 | nav_msgs 51 | pluginlib 52 | roscpp 53 | std_msgs 54 | std_srvs 55 | tf 56 | DEPENDS Boost 57 | ) 58 | 59 | include_directories( 60 | include 61 | ${catkin_INCLUDE_DIRS} 62 | ${Boost_INCLUDE_DIRS} 63 | ) 64 | 65 | add_library(${MBF_NAV_CORE_WRAPPER_LIB} 66 | src/nav_core_wrapper/wrapper_global_planner.cpp 67 | src/nav_core_wrapper/wrapper_local_planner.cpp 68 | src/nav_core_wrapper/wrapper_recovery_behavior.cpp 69 | ) 70 | add_dependencies(${MBF_NAV_CORE_WRAPPER_LIB} ${catkin_EXPORTED_TARGETS}) 71 | target_link_libraries(${MBF_NAV_CORE_WRAPPER_LIB} 72 | ${catkin_LIBRARIES} 73 | ${Boost_LIBRARIES} 74 | ) 75 | 76 | add_library(${MBF_COSTMAP_2D_SERVER_LIB} 77 | src/mbf_costmap_nav/costmap_navigation_server.cpp 78 | src/mbf_costmap_nav/costmap_planner_execution.cpp 79 | src/mbf_costmap_nav/costmap_controller_execution.cpp 80 | src/mbf_costmap_nav/costmap_recovery_execution.cpp 81 | src/mbf_costmap_nav/costmap_wrapper.cpp 82 | src/mbf_costmap_nav/footprint_helper.cpp 83 | src/mbf_costmap_nav/free_pose_search.cpp 84 | src/mbf_costmap_nav/free_pose_search_viz.cpp 85 | ) 86 | add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${catkin_EXPORTED_TARGETS}) 87 | add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${MBF_NAV_CORE_WRAPPER_LIB}) 88 | add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${PROJECT_NAME}_gencfg) 89 | 90 | target_link_libraries(${MBF_COSTMAP_2D_SERVER_LIB} 91 | ${MBF_NAV_CORE_WRAPPER_LIB} 92 | ${catkin_LIBRARIES} 93 | ${Boost_LIBRARIES} 94 | ) 95 | 96 | add_executable(${MBF_COSTMAP_2D_SERVER_NODE} src/move_base_server_node.cpp) 97 | add_dependencies(${MBF_COSTMAP_2D_SERVER_NODE} ${MBF_COSTMAP_2D_SERVER_LIB}) 98 | target_link_libraries(${MBF_COSTMAP_2D_SERVER_NODE} 99 | ${MBF_COSTMAP_2D_SERVER_LIB} 100 | ${catkin_LIBRARIES} 101 | ) 102 | 103 | install(TARGETS 104 | ${MBF_NAV_CORE_WRAPPER_LIB} ${MBF_COSTMAP_2D_SERVER_LIB} ${MBF_COSTMAP_2D_SERVER_NODE} 105 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 106 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 107 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 108 | ) 109 | 110 | install(DIRECTORY include/${PROJECT_NAME}/ 111 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 112 | ) 113 | 114 | catkin_install_python(PROGRAMS scripts/move_base_legacy_relay.py 115 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 116 | ) 117 | 118 | ############# 119 | ## Test ## 120 | ############# 121 | 122 | if(CATKIN_ENABLE_TESTING) 123 | find_package(rostest REQUIRED) 124 | find_package(map_server REQUIRED) 125 | 126 | add_rostest_gtest(free_pose_search_test 127 | test/free_pose_search.test 128 | test/free_pose_search_test.cpp 129 | ) 130 | target_link_libraries(free_pose_search_test 131 | ${MBF_COSTMAP_2D_SERVER_LIB} 132 | ) 133 | endif() 134 | -------------------------------------------------------------------------------- /mbf_costmap_nav/README.md: -------------------------------------------------------------------------------- 1 | # Move Base Flex Costmap Navigation Server {#mainpage} 2 | 3 | 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. 4 | 5 | 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. 6 | 7 | 8 | ![mbf_abstract_nav sketch](doc/images/mbf_costmap_nav_s.png) 9 | -------------------------------------------------------------------------------- /mbf_costmap_nav/cfg/MoveBaseFlex.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'mbf_costmap_nav' 4 | 5 | from mbf_abstract_nav import add_mbf_abstract_nav_params 6 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t 7 | 8 | gen = ParameterGenerator() 9 | 10 | # include the abstract configuration common to all MBF-based navigation plus costmap navigation specific parameters 11 | add_mbf_abstract_nav_params(gen) 12 | 13 | gen.add("shutdown_costmaps", bool_t, 0, 14 | "Determines whether or not to shutdown the costmaps of the node when move_base_flex is in an inactive state", 15 | False) 16 | gen.add("shutdown_costmaps_delay", double_t, 0, 17 | "How long in seconds to wait after last action before shutting down the costmaps", 1.0, 0, 60) 18 | 19 | gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False) 20 | 21 | exit(gen.generate(PACKAGE, "mbf_costmap_nav", "MoveBaseFlex")) 22 | -------------------------------------------------------------------------------- /mbf_costmap_nav/doc/images/mbf_costmap_nav.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/move_base_flex/367269c3c2427dfee29426e5f9f136731fb3b049/mbf_costmap_nav/doc/images/mbf_costmap_nav.png -------------------------------------------------------------------------------- /mbf_costmap_nav/doc/images/mbf_costmap_nav_s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/move_base_flex/367269c3c2427dfee29426e5f9f136731fb3b049/mbf_costmap_nav/doc/images/mbf_costmap_nav_s.png -------------------------------------------------------------------------------- /mbf_costmap_nav/include/mbf_costmap_nav/costmap_recovery_execution.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * costmap_recovery_execution.h 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_COSTMAP_NAV__COSTMAP_RECOVERY_EXECUTION_H_ 42 | #define MBF_COSTMAP_NAV__COSTMAP_RECOVERY_EXECUTION_H_ 43 | 44 | #include 45 | #include 46 | 47 | #include "mbf_costmap_nav/MoveBaseFlexConfig.h" 48 | #include "mbf_costmap_nav/costmap_wrapper.h" 49 | 50 | 51 | namespace mbf_costmap_nav 52 | { 53 | /** 54 | * @brief The CostmapRecoveryExecution binds a local and a global costmap to the AbstractRecoveryExecution and uses the 55 | * nav_core/CostmapRecovery class as base plugin interface. 56 | * This class makes move_base_flex compatible to the old move_base. 57 | * 58 | * @ingroup recovery_execution move_base_server 59 | */ 60 | class CostmapRecoveryExecution : public mbf_abstract_nav::AbstractRecoveryExecution 61 | { 62 | 63 | public: 64 | typedef boost::shared_ptr Ptr; 65 | 66 | /** 67 | * @brief Constructor. 68 | * @param recovery_name Name of the recovery behavior to run. 69 | * @param recovery_ptr Shared pointer to the plugin to use. 70 | * @param robot_info Current robot state 71 | * @param global_costmap Shared pointer to the global costmap. 72 | * @param local_costmap Shared pointer to the local costmap. 73 | * @param config Current server configuration (dynamic). 74 | */ 75 | CostmapRecoveryExecution( 76 | const std::string &recovery_name, 77 | const mbf_costmap_core::CostmapRecovery::Ptr &recovery_ptr, 78 | const mbf_utility::RobotInformation &robot_info, 79 | const CostmapWrapper::Ptr &global_costmap, 80 | const CostmapWrapper::Ptr &local_costmap, 81 | const MoveBaseFlexConfig &config); 82 | 83 | /** 84 | * Destructor 85 | */ 86 | virtual ~CostmapRecoveryExecution(); 87 | 88 | private: 89 | /** 90 | * @brief Implementation-specific setup function, called right before execution. 91 | * This method overrides abstract execution empty implementation with underlying map-specific setup code. 92 | */ 93 | void preRun() 94 | { 95 | local_costmap_->checkActivate(); 96 | global_costmap_->checkActivate(); 97 | }; 98 | 99 | /** 100 | * @brief Implementation-specific cleanup function, called right after execution. 101 | * This method overrides abstract execution empty implementation with underlying map-specific cleanup code. 102 | */ 103 | void postRun() 104 | { 105 | local_costmap_->checkDeactivate(); 106 | global_costmap_->checkDeactivate(); 107 | }; 108 | 109 | mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config); 110 | 111 | //! Shared pointer to the global costmap 112 | const CostmapWrapper::Ptr &global_costmap_; 113 | 114 | //! Shared pointer to thr local costmap 115 | const CostmapWrapper::Ptr &local_costmap_; 116 | }; 117 | 118 | } /* namespace mbf_costmap_nav */ 119 | 120 | #endif /* MBF_COSTMAP_NAV__COSTMAP_RECOVERY_EXECUTION_H_ */ 121 | -------------------------------------------------------------------------------- /mbf_costmap_nav/include/mbf_costmap_nav/costmap_wrapper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * costmap_wrapper.h 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_COSTMAP_NAV__COSTMAP_WRAPPER_H_ 42 | #define MBF_COSTMAP_NAV__COSTMAP_WRAPPER_H_ 43 | 44 | #include 45 | 46 | #include 47 | 48 | 49 | namespace mbf_costmap_nav 50 | { 51 | /** 52 | * @defgroup move_base_server Move Base Server 53 | * @brief Classes belonging to the Move Base Server level. 54 | */ 55 | 56 | 57 | /** 58 | * @brief The CostmapWrapper class manages access to a costmap by locking/unlocking its mutex and handles 59 | * (de)activation. 60 | * 61 | * @ingroup navigation_server move_base_server 62 | */ 63 | class CostmapWrapper : public costmap_2d::Costmap2DROS 64 | { 65 | public: 66 | typedef boost::shared_ptr Ptr; 67 | 68 | /** 69 | * @brief Constructor 70 | * @param tf_listener_ptr Shared pointer to a common TransformListener 71 | */ 72 | CostmapWrapper(const std::string &name, const TFPtr &tf_listener_ptr); 73 | 74 | /** 75 | * @brief Destructor 76 | */ 77 | virtual ~CostmapWrapper(); 78 | 79 | /** 80 | * @brief Reconfiguration method called by dynamic reconfigure. 81 | * @param shutdown_costmap Determines whether or not to shutdown the costmap when move_base_flex is inactive. 82 | * @param shutdown_costmap_delay How long in seconds to wait after last action before shutting down the costmap. 83 | */ 84 | void reconfigure(double shutdown_costmap, double shutdown_costmap_delay); 85 | 86 | /** 87 | * @brief Clear costmap. 88 | */ 89 | void clear(); 90 | 91 | /** 92 | * @brief Check whether the costmap should be activated. 93 | */ 94 | void checkActivate(); 95 | 96 | /** 97 | * @brief Check whether the costmap should and could be deactivated. 98 | */ 99 | void checkDeactivate(); 100 | 101 | private: 102 | /** 103 | * @brief Timer-triggered deactivation of the costmap. 104 | */ 105 | void deactivate(const ros::TimerEvent &event); 106 | 107 | //! Private node handle 108 | ros::NodeHandle private_nh_; 109 | 110 | boost::mutex check_costmap_mutex_; //!< Start/stop costmap mutex; concurrent calls to start can lead to segfault 111 | bool shutdown_costmap_; //!< don't update costmap when not using it 112 | bool clear_on_shutdown_; //!< clear the costmap, when shutting down 113 | int16_t costmap_users_; //!< keep track of plugins using costmap 114 | ros::Timer shutdown_costmap_timer_; //!< costmap delayed shutdown timer 115 | ros::Duration shutdown_costmap_delay_; //!< costmap delayed shutdown delay 116 | }; 117 | 118 | } /* namespace mbf_costmap_nav */ 119 | 120 | #endif /* MBF_COSTMAP_NAV__COSTMAP_WRAPPER_H_ */ 121 | -------------------------------------------------------------------------------- /mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: TKruse 36 | *********************************************************************/ 37 | 38 | #ifndef FOOTPRINT_HELPER_H_ 39 | #define FOOTPRINT_HELPER_H_ 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | namespace mbf_costmap_nav 47 | { 48 | 49 | struct Cell 50 | { 51 | unsigned int x, y, cost; 52 | }; 53 | 54 | class FootprintHelper 55 | { 56 | public: 57 | /** 58 | * @brief Used to get the cells that make up the footprint of the robot 59 | * @param x The x position of the robot 60 | * @param y The y position of the robot 61 | * @param theta The orientation of the robot 62 | * @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. 63 | * @return The cells that make up either the outline or entire footprint of the robot depending on fill 64 | */ 65 | static std::vector getFootprintCells(double x, double y, double theta, 66 | const std::vector& footprint_spec, 67 | const costmap_2d::Costmap2D&, bool fill); 68 | 69 | /** 70 | * @brief Supercover algorithm is a modified Bresenham which prints ALL the points (not only one point per axis) the ideal line contains 71 | * ref: http://eugen.dedu.free.fr/projects/bresenham/ 72 | * @param x0 The x coordinate of the first point 73 | * @param x1 The x coordinate of the second point 74 | * @param y0 The y coordinate of the first point 75 | * @param y1 The y coordinate of the second point 76 | * @param pts Will be filled with the cells that lie on the line in the grid 77 | */ 78 | static void supercover(int x0, int x1, int y0, int y1, std::vector& pts); 79 | 80 | /** 81 | * @brief Use Bresenham's algorithm to trace a line between two points in a grid 82 | * @param x0 The x coordinate of the first point 83 | * @param x1 The x coordinate of the second point 84 | * @param y0 The y coordinate of the first point 85 | * @param y1 The y coordinate of the second point 86 | * @param pts Will be filled with the cells that lie on the line in the grid 87 | */ 88 | static void getLineCells(int x0, int x1, int y0, int y1, std::vector& pts); 89 | 90 | /** 91 | * @brief Fill the outline of a polygon, in this case the robot footprint, in a grid 92 | * @param footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint 93 | */ 94 | static void getFillCells(std::vector& footprint); 95 | }; 96 | 97 | } /* namespace mbf_costmap_nav */ 98 | #endif /* FOOTPRINT_HELPER_H_ */ 99 | -------------------------------------------------------------------------------- /mbf_costmap_nav/include/mbf_costmap_nav/free_pose_search_viz.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2023, Rapyuta Robotics Co., Ltd., Renan Salles 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * free_pose_search_viz.h 34 | * 35 | * authors: 36 | * Renan Salles 37 | * 38 | */ 39 | 40 | #ifndef SEARCH_HELPER_VIZ_H_ 41 | #define SEARCH_HELPER_VIZ_H_ 42 | 43 | // ros 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | namespace mbf_costmap_nav 51 | { 52 | class FreePoseSearchViz 53 | { 54 | private: 55 | using IdType = visualization_msgs::Marker::_id_type; 56 | static constexpr auto BLOCKED_NS = "blocked_footprints"; 57 | static constexpr auto SOLUTION_NS = "solution"; 58 | 59 | ros::NodeHandle& pnh_; 60 | std::string frame_id_; 61 | 62 | IdType marker_id_{ 0 }; 63 | visualization_msgs::MarkerArray marker_array_; 64 | ros::Publisher marker_pub_; 65 | 66 | void addMarker(const geometry_msgs::Pose2D& pose_2d, const std::vector& footprint, 67 | const std::string& ns, const std_msgs::ColorRGBA& color); 68 | 69 | public: 70 | FreePoseSearchViz(ros::NodeHandle& pnh, const std::string& frame_id); 71 | 72 | /** 73 | * @brief It adds a red marker for a footprint that is blocked by the obstacle in namespace "blocked_footprints" 74 | * @param pose_2d pose of the footprint 75 | * @param frame_id frame of the pose 76 | * @param footprint the footprint 77 | */ 78 | void addBlocked(const geometry_msgs::Pose2D& pose_2d, const std::vector& footprint); 79 | 80 | /** 81 | * @brief It adds a green marker for a footprint that is not blocked by the obstacle in namespace "solution" 82 | * @param pose_2d pose of the footprint 83 | * @param frame_id frame of the pose 84 | * @param footprint the footprint 85 | */ 86 | void addSolution(const geometry_msgs::Pose2D& pose_2d, const std::vector& footprint); 87 | 88 | /** 89 | * @brief It clear the previous viz and publishes the markers 90 | */ 91 | void publish(); 92 | 93 | /** 94 | * @brief It clears the previous viz 95 | */ 96 | void deleteMarkers(); 97 | }; 98 | 99 | } /* namespace mbf_costmap_nav */ 100 | #endif /* SEARCH_HELPER_VIZ_H_ */ 101 | -------------------------------------------------------------------------------- /mbf_costmap_nav/include/nav_core_wrapper/wrapper_global_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * wrapper_global_planner.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_COSTMAP_NAV__WRAPPER_GLOBAL_PLANNER_H_ 42 | #define MBF_COSTMAP_NAV__WRAPPER_GLOBAL_PLANNER_H_ 43 | 44 | #include 45 | #include "mbf_costmap_core/costmap_planner.h" 46 | 47 | namespace mbf_nav_core_wrapper { 48 | /** 49 | * @class CostmapPlanner 50 | * @brief Provides an interface for global planners used in navigation. 51 | * All global planners written to work as MBF plugins must adhere to this interface. Alternatively, this 52 | * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility. 53 | */ 54 | class WrapperGlobalPlanner : public mbf_costmap_core::CostmapPlanner{ 55 | public: 56 | 57 | /** 58 | * @brief Given a goal pose in the world, compute a plan 59 | * @param start The start pose 60 | * @param goal The goal pose 61 | * @param tolerance If the goal is obstructed, how many meters the planner can relax the constraint 62 | * in x and y before failing 63 | * @param plan The plan... filled by the planner 64 | * @param cost The cost for the the plan 65 | * @param message Optional more detailed outcome as a string 66 | * @return Result code as described on GetPath action result, As this is a wrapper to the nav_core, 67 | * only 0 (SUCCESS) and 50 (FAILURE) are supported 68 | */ 69 | virtual uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, 70 | double tolerance, std::vector &plan, double &cost, 71 | std::string &message); 72 | 73 | /** 74 | * @brief Requests the planner to cancel, e.g. if it takes too much time. 75 | * @remark New on MBF API 76 | * @return True if a cancel has been successfully requested, false if not implemented. 77 | */ 78 | virtual bool cancel(); 79 | 80 | /** 81 | * @brief Initialization function for the CostmapPlanner 82 | * @param name The name of this planner 83 | * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning 84 | */ 85 | virtual void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros); 86 | 87 | /** 88 | * @brief Public constructor used for handling a nav_core-based plugin 89 | * @param plugin Backward compatible plugin 90 | */ 91 | WrapperGlobalPlanner(boost::shared_ptr< nav_core::BaseGlobalPlanner > plugin); 92 | 93 | /** 94 | * @brief Virtual destructor for the interface 95 | */ 96 | virtual ~WrapperGlobalPlanner(); 97 | 98 | private: 99 | boost::shared_ptr< nav_core::BaseGlobalPlanner > nav_core_plugin_; 100 | }; 101 | } /* namespace mbf_nav_core_wrapper */ 102 | 103 | #endif /* MBF_COSTMAP_NAV__WRAPPER_GLOBAL_PLANNER_H_ */ 104 | -------------------------------------------------------------------------------- /mbf_costmap_nav/include/nav_core_wrapper/wrapper_recovery_behavior.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * wrapper_recovery_behavior.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_COSTMAP_NAV__WRAPPER_RECOVERY_BEHAVIOR_H_ 42 | #define MBF_COSTMAP_NAV__WRAPPER_RECOVERY_BEHAVIOR_H_ 43 | 44 | #include 45 | #include "mbf_costmap_core/costmap_recovery.h" 46 | 47 | #include 48 | namespace mbf_nav_core_wrapper { 49 | /** 50 | * @class CostmapRecovery 51 | * @brief Provides an interface for recovery behaviors used in navigation. 52 | * All recovery behaviors written to work as MBF plugins must adhere to this interface. Alternatively, this 53 | * class can also operate as a wrapper for old API nav_core-based plugins, providing backward compatibility. 54 | */ 55 | class WrapperRecoveryBehavior : public mbf_costmap_core::CostmapRecovery{ 56 | public: 57 | 58 | /** 59 | * @brief Initialization function for the CostmapRecovery 60 | * @param tf A pointer to a transform listener 61 | * @param global_costmap A pointer to the global_costmap used by the navigation stack 62 | * @param local_costmap A pointer to the local_costmap used by the navigation stack 63 | */ 64 | virtual void initialize(std::string name, TF* tf, 65 | costmap_2d::Costmap2DROS* global_costmap, 66 | costmap_2d::Costmap2DROS* local_costmap); 67 | 68 | /** 69 | * @brief Runs the CostmapRecovery 70 | */ 71 | virtual uint32_t runBehavior(std::string &message); 72 | 73 | /** 74 | * @brief Requests the planner to cancel, e.g. if it takes too much time 75 | * @remark New on MBF API 76 | * @return True if a cancel has been successfully requested, false if not implemented. 77 | */ 78 | virtual bool cancel(); 79 | 80 | /** 81 | * @brief Public constructor used for handling a nav_core-based plugin 82 | * @param plugin Backward compatible plugin 83 | */ 84 | WrapperRecoveryBehavior(boost::shared_ptr< nav_core::RecoveryBehavior > plugin); 85 | 86 | /** 87 | * @brief Virtual destructor for the interface 88 | */ 89 | virtual ~WrapperRecoveryBehavior(); 90 | 91 | private: 92 | boost::shared_ptr< nav_core::RecoveryBehavior > nav_core_plugin_; 93 | }; 94 | }; /* namespace mbf_nav_core_wrapper */ 95 | 96 | #endif /* MBF_COSTMAP_NAV__WRAPPER_RECOVERY_BEHAVIOR_H_ */ 97 | -------------------------------------------------------------------------------- /mbf_costmap_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mbf_costmap_nav 3 | 0.4.0 4 | 5 | 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 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. 6 | 7 | Additionally the mbf_costmap_nav package comes with a wrapper for the old navigation stack and the plugins which inherits from the nav_core base classes. Preferably it tries to load plugins for the new API. However, plugins could even support both move_base and move_base_flex by inheriting both base class interfaces located in the nav_core package and in the mbf_costmap_core package. 8 | 9 | http://wiki.ros.org/move_base_flex 10 | Sebastian Pütz 11 | Sebastian Pütz 12 | Jorge Santos 13 | BSD-3 14 | 15 | catkin 16 | 17 | actionlib 18 | actionlib_msgs 19 | angles 20 | costmap_2d 21 | dynamic_reconfigure 22 | geometry_msgs 23 | mbf_abstract_nav 24 | mbf_costmap_core 25 | mbf_msgs 26 | mbf_utility 27 | nav_core 28 | nav_msgs 29 | pluginlib 30 | roscpp 31 | std_msgs 32 | std_srvs 33 | tf 34 | 35 | 36 | move_base 37 | move_base_msgs 38 | 39 | gtest 40 | map_server 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /mbf_costmap_nav/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: Move Base Flex 3 | homepage: http://wiki.ros.org/move_base_flex 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.py *.md' 5 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * costmap_controller_execution.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | #include "mbf_costmap_nav/costmap_controller_execution.h" 41 | 42 | namespace mbf_costmap_nav 43 | { 44 | 45 | CostmapControllerExecution::CostmapControllerExecution(const std::string& controller_name, 46 | const mbf_costmap_core::CostmapController::Ptr& controller_ptr, 47 | const mbf_utility::RobotInformation& robot_info, 48 | const ros::Publisher& vel_pub, 49 | const CostmapWrapper::Ptr& costmap_ptr, 50 | const MoveBaseFlexConfig& config) 51 | : AbstractControllerExecution(controller_name, controller_ptr, robot_info, vel_pub, toAbstract(config)) 52 | , costmap_ptr_(costmap_ptr) 53 | { 54 | ros::NodeHandle private_nh("~"); 55 | private_nh.param("controller_lock_costmap", lock_costmap_, true); 56 | } 57 | 58 | CostmapControllerExecution::~CostmapControllerExecution() 59 | { 60 | } 61 | 62 | mbf_abstract_nav::MoveBaseFlexConfig CostmapControllerExecution::toAbstract(const MoveBaseFlexConfig &config) 63 | { 64 | // copy the controller-related abstract configuration common to all MBF-based navigation 65 | mbf_abstract_nav::MoveBaseFlexConfig abstract_config; 66 | abstract_config.controller_frequency = config.controller_frequency; 67 | abstract_config.controller_patience = config.controller_patience; 68 | abstract_config.controller_max_retries = config.controller_max_retries; 69 | abstract_config.oscillation_timeout = config.oscillation_timeout; 70 | abstract_config.oscillation_distance = config.oscillation_distance; 71 | abstract_config.oscillation_angle = config.oscillation_angle; 72 | return abstract_config; 73 | } 74 | 75 | uint32_t CostmapControllerExecution::computeVelocityCmd( 76 | const geometry_msgs::PoseStamped &robot_pose, 77 | const geometry_msgs::TwistStamped &robot_velocity, 78 | geometry_msgs::TwistStamped &vel_cmd, 79 | std::string &message) 80 | { 81 | // Lock the costmap while planning, but following issue #4, we allow to move the responsibility to the planner itself 82 | if (lock_costmap_) 83 | { 84 | boost::lock_guard lock(*(costmap_ptr_->getCostmap()->getMutex())); 85 | return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message); 86 | } 87 | return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message); 88 | } 89 | 90 | bool CostmapControllerExecution::safetyCheck() 91 | { 92 | // Check that the observation buffers for the costmap are current, we don't want to drive blind 93 | if (!costmap_ptr_->isCurrent()) 94 | { 95 | ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety"); 96 | return false; 97 | } 98 | return true; 99 | } 100 | 101 | } /* namespace mbf_costmap_nav */ 102 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/mbf_costmap_nav/costmap_planner_execution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * costmap_planner_execution.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | #include 41 | #include 42 | 43 | #include "mbf_costmap_nav/costmap_planner_execution.h" 44 | 45 | namespace mbf_costmap_nav 46 | { 47 | CostmapPlannerExecution::CostmapPlannerExecution(const std::string& planner_name, 48 | const mbf_costmap_core::CostmapPlanner::Ptr& planner_ptr, 49 | const mbf_utility::RobotInformation& robot_info, 50 | const CostmapWrapper::Ptr& costmap_ptr, 51 | const MoveBaseFlexConfig& config) 52 | : AbstractPlannerExecution(planner_name, planner_ptr, robot_info, toAbstract(config)), costmap_ptr_(costmap_ptr) 53 | { 54 | ros::NodeHandle private_nh("~"); 55 | private_nh.param("planner_lock_costmap", lock_costmap_, true); 56 | } 57 | 58 | CostmapPlannerExecution::~CostmapPlannerExecution() 59 | { 60 | } 61 | 62 | mbf_abstract_nav::MoveBaseFlexConfig CostmapPlannerExecution::toAbstract(const MoveBaseFlexConfig &config) 63 | { 64 | // copy the planner-related abstract configuration common to all MBF-based navigation 65 | mbf_abstract_nav::MoveBaseFlexConfig abstract_config; 66 | abstract_config.planner_frequency = config.planner_frequency; 67 | abstract_config.planner_patience = config.planner_patience; 68 | abstract_config.planner_max_retries = config.planner_max_retries; 69 | return abstract_config; 70 | } 71 | 72 | uint32_t CostmapPlannerExecution::makePlan(const geometry_msgs::PoseStamped &start, 73 | const geometry_msgs::PoseStamped &goal, 74 | double tolerance, 75 | std::vector &plan, 76 | double &cost, 77 | std::string &message) 78 | { 79 | // transform the input to the global frame of the costmap, since this is an 80 | // "implicit" requirement for most planners 81 | // note: costmap_2d::Costmap2DROS::getTransformTolerance might be a good idea, 82 | // but it's not part of the class API in ros-kinetic 83 | const ros::Duration timeout(0.5); 84 | const std::string frame = costmap_ptr_->getGlobalFrameID(); 85 | geometry_msgs::PoseStamped g_start, g_goal; 86 | 87 | if (!mbf_utility::transformPose(robot_info_.getTransformListener(), frame, timeout, start, g_start)) 88 | return mbf_msgs::GetPathResult::TF_ERROR; 89 | 90 | if (!mbf_utility::transformPose(robot_info_.getTransformListener(), frame, timeout, goal, g_goal)) 91 | return mbf_msgs::GetPathResult::TF_ERROR; 92 | 93 | if (lock_costmap_) 94 | { 95 | boost::unique_lock lock(*(costmap_ptr_->getCostmap()->getMutex())); 96 | return planner_->makePlan(g_start, g_goal, tolerance, plan, cost, message); 97 | } 98 | return planner_->makePlan(g_start, g_goal, tolerance, plan, cost, message); 99 | } 100 | 101 | } /* namespace mbf_costmap_nav */ 102 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/mbf_costmap_nav/costmap_recovery_execution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * costmap_recovery_execution.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | #include 41 | #include "nav_core_wrapper/wrapper_recovery_behavior.h" 42 | #include "mbf_costmap_nav/costmap_recovery_execution.h" 43 | 44 | namespace mbf_costmap_nav 45 | { 46 | 47 | CostmapRecoveryExecution::CostmapRecoveryExecution( 48 | const std::string &recovery_name, 49 | const mbf_costmap_core::CostmapRecovery::Ptr &recovery_ptr, 50 | const mbf_utility::RobotInformation &robot_info, 51 | const CostmapWrapper::Ptr &global_costmap, 52 | const CostmapWrapper::Ptr &local_costmap, 53 | const MoveBaseFlexConfig &config) 54 | : AbstractRecoveryExecution(recovery_name, recovery_ptr, robot_info, toAbstract(config)), 55 | global_costmap_(global_costmap), local_costmap_(local_costmap) 56 | { 57 | } 58 | 59 | CostmapRecoveryExecution::~CostmapRecoveryExecution() 60 | { 61 | } 62 | 63 | mbf_abstract_nav::MoveBaseFlexConfig CostmapRecoveryExecution::toAbstract(const MoveBaseFlexConfig &config) 64 | { 65 | // copy the recovery-related abstract configuration common to all MBF-based navigation 66 | mbf_abstract_nav::MoveBaseFlexConfig abstract_config; 67 | abstract_config.recovery_enabled = config.recovery_enabled; 68 | abstract_config.recovery_patience = config.recovery_patience; 69 | return abstract_config; 70 | } 71 | 72 | } /* namespace mbf_costmap_nav */ 73 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * costmap_wrapper.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #include "mbf_costmap_nav/costmap_wrapper.h" 42 | 43 | 44 | namespace mbf_costmap_nav 45 | { 46 | 47 | 48 | CostmapWrapper::CostmapWrapper(const std::string &name, const TFPtr &tf_listener_ptr) : 49 | costmap_2d::Costmap2DROS(name, *tf_listener_ptr), 50 | shutdown_costmap_(false), costmap_users_(0), private_nh_("~") 51 | { 52 | // even if shutdown_costmaps is a dynamically reconfigurable parameter, we 53 | // need it here to decide whether to start or not the costmap on starting up 54 | private_nh_.param("shutdown_costmaps", shutdown_costmap_, false); 55 | private_nh_.param("clear_on_shutdown", clear_on_shutdown_, false); 56 | 57 | if (shutdown_costmap_) 58 | // initialize costmap stopped if shutdown_costmaps parameter is true 59 | stop(); 60 | else 61 | // otherwise costmap_users_ is at least 1, as costmap is always active 62 | ++costmap_users_; 63 | } 64 | 65 | CostmapWrapper::~CostmapWrapper() 66 | { 67 | shutdown_costmap_timer_.stop(); 68 | } 69 | 70 | 71 | void CostmapWrapper::reconfigure(double shutdown_costmap, double shutdown_costmap_delay) 72 | { 73 | shutdown_costmap_delay_ = ros::Duration(shutdown_costmap_delay); 74 | if (shutdown_costmap_delay_.isZero()) 75 | ROS_WARN("Zero shutdown costmaps delay is not recommended, as it forces us to enable costmaps on each action"); 76 | 77 | if (shutdown_costmap_ && !shutdown_costmap) 78 | { 79 | checkActivate(); 80 | shutdown_costmap_ = shutdown_costmap; 81 | } 82 | if (!shutdown_costmap_ && shutdown_costmap) 83 | { 84 | shutdown_costmap_ = shutdown_costmap; 85 | checkDeactivate(); 86 | } 87 | } 88 | 89 | void CostmapWrapper::clear() 90 | { 91 | // lock and clear costmap 92 | boost::unique_lock lock(*getCostmap()->getMutex()); 93 | resetLayers(); 94 | } 95 | 96 | void CostmapWrapper::checkActivate() 97 | { 98 | boost::mutex::scoped_lock sl(check_costmap_mutex_); 99 | 100 | shutdown_costmap_timer_.stop(); 101 | 102 | // Activate costmap if we shutdown them when not moving and they are not already active. This method must be 103 | // synchronized because start costmap can take up to 1/update freq., and concurrent calls to it can lead to segfaults 104 | if (shutdown_costmap_ && !costmap_users_) 105 | { 106 | start(); 107 | ROS_DEBUG_STREAM("" << name_ << " activated"); 108 | } 109 | ++costmap_users_; 110 | } 111 | 112 | void CostmapWrapper::checkDeactivate() 113 | { 114 | boost::mutex::scoped_lock sl(check_costmap_mutex_); 115 | 116 | --costmap_users_; 117 | ROS_ASSERT_MSG(costmap_users_ >= 0, "Negative number (%d) of active users count!", costmap_users_); 118 | if (shutdown_costmap_ && !costmap_users_) 119 | { 120 | // Delay costmap shutdown by shutdown_costmap_delay so we don't need to enable at each step of a normal 121 | // navigation sequence, what is terribly inefficient; the timer is stopped on costmap re-activation and 122 | // reset after every new call to deactivate 123 | shutdown_costmap_timer_ = 124 | private_nh_.createTimer(shutdown_costmap_delay_, &CostmapWrapper::deactivate, this, true); 125 | } 126 | } 127 | 128 | void CostmapWrapper::deactivate(const ros::TimerEvent &event) 129 | { 130 | boost::mutex::scoped_lock sl(check_costmap_mutex_); 131 | 132 | ROS_ASSERT_MSG(!costmap_users_, "Deactivating costmap with %d active users!", costmap_users_); 133 | if (clear_on_shutdown_) 134 | clear(); // do before stop, as some layers (e.g. obstacle and voxel) reactivate their subscribers on reset 135 | stop(); 136 | ROS_DEBUG_STREAM("" << name_ << " deactivated"); 137 | } 138 | 139 | } /* namespace mbf_costmap_nav */ 140 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/mbf_costmap_nav/free_pose_search_viz.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2023, Rapyuta Robotics Co., Ltd., Renan Salles 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * free_pose_search_viz.cpp 34 | * 35 | * authors: 36 | * Renan Salles 37 | * 38 | */ 39 | 40 | // mbf 41 | #include "mbf_costmap_nav/free_pose_search_viz.h" 42 | 43 | // ros 44 | #include 45 | #include 46 | 47 | namespace mbf_costmap_nav 48 | { 49 | FreePoseSearchViz::FreePoseSearchViz(ros::NodeHandle& pnh, const std::string& frame_id) 50 | : pnh_(pnh) 51 | , frame_id_(frame_id) 52 | , marker_pub_(pnh_.advertise("search_markers", 1, false)) 53 | { 54 | } 55 | 56 | void FreePoseSearchViz::addMarker(const geometry_msgs::Pose2D& pose_2d, 57 | const std::vector& footprint, const std::string& ns, 58 | const std_msgs::ColorRGBA& color) 59 | { 60 | tf2::Quaternion q; 61 | q.setRPY(0, 0, pose_2d.theta); 62 | 63 | visualization_msgs::Marker marker; 64 | marker.header.frame_id = frame_id_; 65 | marker.header.stamp = ros::Time::now(); 66 | marker.ns = ns; 67 | marker.id = marker_id_++; 68 | marker.type = visualization_msgs::Marker::LINE_STRIP; 69 | marker.action = visualization_msgs::Marker::ADD; 70 | marker.pose.position.x = pose_2d.x; 71 | marker.pose.position.y = pose_2d.y; 72 | marker.pose.position.z = 0; 73 | marker.pose.orientation = tf2::toMsg(q); 74 | marker.scale.x = 0.05; 75 | marker.color = color; 76 | marker.lifetime = ros::Duration(0); 77 | 78 | for (const auto& point : footprint) 79 | { 80 | geometry_msgs::Point p; 81 | p.x = point.x; 82 | p.y = point.y; 83 | p.z = 0; 84 | marker.points.push_back(p); 85 | } 86 | marker.points.push_back(marker.points.front()); 87 | marker_array_.markers.push_back(marker); 88 | } 89 | 90 | void FreePoseSearchViz::deleteMarkers() 91 | { 92 | visualization_msgs::MarkerArray marker_array; 93 | visualization_msgs::Marker marker; 94 | marker.action = visualization_msgs::Marker::DELETEALL; 95 | marker.ns = BLOCKED_NS; 96 | marker_array.markers.push_back(marker); 97 | marker.ns = SOLUTION_NS; 98 | marker_array.markers.push_back(marker); 99 | marker_pub_.publish(marker_array); 100 | } 101 | 102 | void FreePoseSearchViz::addBlocked(const geometry_msgs::Pose2D& pose_2d, 103 | const std::vector& footprint) 104 | { 105 | std_msgs::ColorRGBA color; 106 | color.r = 1; 107 | color.a = 0.5; 108 | addMarker(pose_2d, footprint, BLOCKED_NS, color); 109 | } 110 | 111 | void FreePoseSearchViz::addSolution(const geometry_msgs::Pose2D& pose_2d, 112 | const std::vector& footprint) 113 | { 114 | std_msgs::ColorRGBA color; 115 | color.g = 1; 116 | color.a = 0.5; 117 | addMarker(pose_2d, footprint, SOLUTION_NS, color); 118 | } 119 | 120 | void FreePoseSearchViz::publish() 121 | { 122 | marker_pub_.publish(marker_array_); 123 | ROS_DEBUG("Published %zu markers", marker_array_.markers.size()); 124 | marker_array_.markers.clear(); 125 | marker_id_ = 0; 126 | } 127 | } // namespace mbf_costmap_nav 128 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/move_base_server_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * move_base_server_node.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #include "mbf_costmap_nav/costmap_navigation_server.h" 42 | #include 43 | #include 44 | #include 45 | 46 | typedef boost::shared_ptr CostmapNavigationServerPtr; 47 | mbf_costmap_nav::CostmapNavigationServer::Ptr costmap_nav_srv_ptr; 48 | 49 | void sigintHandler(int sig) 50 | { 51 | ROS_INFO_STREAM("Shutdown costmap navigation server."); 52 | if(costmap_nav_srv_ptr) 53 | { 54 | costmap_nav_srv_ptr->stop(); 55 | } 56 | ros::shutdown(); 57 | } 58 | 59 | int main(int argc, char **argv) 60 | { 61 | ros::init(argc, argv, "mbf_2d_nav_server", ros::init_options::NoSigintHandler); 62 | 63 | ros::NodeHandle nh; 64 | ros::NodeHandle private_nh("~"); 65 | 66 | double cache_time; 67 | private_nh.param("tf_cache_time", cache_time, 10.0); 68 | 69 | signal(SIGINT, sigintHandler); 70 | #ifdef USE_OLD_TF 71 | TFPtr tf_listener_ptr(new TF(nh, ros::Duration(cache_time), true)); 72 | #else 73 | TFPtr tf_listener_ptr(new TF(ros::Duration(cache_time))); 74 | tf2_ros::TransformListener tf_listener(*tf_listener_ptr); 75 | #endif 76 | costmap_nav_srv_ptr = boost::make_shared(tf_listener_ptr); 77 | ros::spin(); 78 | 79 | // explicitly call destructor here, otherwise costmap_nav_srv_ptr will be 80 | // destructed after tearing down internally allocated static variables 81 | costmap_nav_srv_ptr.reset(); 82 | return EXIT_SUCCESS; 83 | } 84 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/nav_core_wrapper/wrapper_global_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * wrapper_global_planner.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #include "nav_core_wrapper/wrapper_global_planner.h" 42 | 43 | namespace mbf_nav_core_wrapper 44 | { 45 | 46 | uint32_t WrapperGlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start, 47 | const geometry_msgs::PoseStamped &goal, 48 | double tolerance, 49 | std::vector &plan, 50 | double &cost, 51 | std::string &message) 52 | { 53 | #if ROS_VERSION_MINIMUM(1, 12, 0) // if current ros version is >= 1.12.0 54 | // Kinetic and beyond 55 | bool success = nav_core_plugin_->makePlan(start, goal, plan, cost); 56 | #else 57 | // Indigo 58 | bool success = nav_core_plugin_->makePlan(start, goal, plan); 59 | cost = 0; 60 | #endif 61 | message = success ? "Plan found" : "Planner failed"; 62 | return success ? 0 : 50; // SUCCESS | FAILURE 63 | } 64 | 65 | bool WrapperGlobalPlanner::cancel() 66 | { 67 | return false; 68 | } 69 | 70 | void WrapperGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) 71 | { 72 | nav_core_plugin_->initialize(name, costmap_ros); 73 | } 74 | 75 | WrapperGlobalPlanner::WrapperGlobalPlanner(boost::shared_ptr plugin) 76 | : nav_core_plugin_(plugin) 77 | {} 78 | 79 | WrapperGlobalPlanner::~WrapperGlobalPlanner() 80 | {} 81 | 82 | }; /* namespace mbf_abstract_core */ 83 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * wrapper_local_planner.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #include "nav_core_wrapper/wrapper_local_planner.h" 42 | 43 | namespace mbf_nav_core_wrapper 44 | { 45 | 46 | uint32_t WrapperLocalPlanner::computeVelocityCommands( 47 | const geometry_msgs::PoseStamped &robot_pose, 48 | const geometry_msgs::TwistStamped &robot_velocity, 49 | geometry_msgs::TwistStamped &cmd_vel, 50 | std::string &message) 51 | { 52 | bool success = nav_core_plugin_->computeVelocityCommands(cmd_vel.twist); 53 | message = success ? "Valid command" : "Controller failed"; 54 | return success ? 0 : 100; // SUCCESS | FAILURE 55 | } 56 | 57 | bool WrapperLocalPlanner::isGoalReached() 58 | { 59 | return nav_core_plugin_->isGoalReached(); 60 | } 61 | 62 | bool WrapperLocalPlanner::isGoalReached(double xy_tolerance, double yaw_tolerance) 63 | { 64 | return isGoalReached(); 65 | } 66 | 67 | bool WrapperLocalPlanner::setPlan(const std::vector &plan) 68 | { 69 | return nav_core_plugin_->setPlan(plan); 70 | } 71 | 72 | bool WrapperLocalPlanner::cancel() 73 | { 74 | ROS_WARN_STREAM("The cancel method is not implemented. " 75 | "Note: you are running a nav_core based plugin, " 76 | "which is wrapped into the MBF interface."); 77 | return false; 78 | } 79 | 80 | void WrapperLocalPlanner::initialize(std::string name, 81 | TF *tf, 82 | costmap_2d::Costmap2DROS *costmap_ros) 83 | { 84 | nav_core_plugin_->initialize(name, tf, costmap_ros); 85 | } 86 | 87 | WrapperLocalPlanner::WrapperLocalPlanner(boost::shared_ptr plugin) 88 | : nav_core_plugin_(plugin) 89 | {} 90 | 91 | WrapperLocalPlanner::~WrapperLocalPlanner() 92 | {} 93 | 94 | }; /* namespace mbf_abstract_core */ 95 | -------------------------------------------------------------------------------- /mbf_costmap_nav/src/nav_core_wrapper/wrapper_recovery_behavior.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * wrapper_recovery_behavior.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #include 42 | #include "nav_core_wrapper/wrapper_recovery_behavior.h" 43 | 44 | namespace mbf_nav_core_wrapper 45 | { 46 | void WrapperRecoveryBehavior::initialize(std::string name, TF *tf, 47 | costmap_2d::Costmap2DROS *global_costmap, 48 | costmap_2d::Costmap2DROS *local_costmap) 49 | { 50 | nav_core_plugin_->initialize(name, tf, global_costmap, local_costmap); 51 | } 52 | 53 | uint32_t WrapperRecoveryBehavior::runBehavior(std::string &message) 54 | { 55 | nav_core_plugin_->runBehavior(); 56 | // TODO return a code for old API 57 | return mbf_msgs::RecoveryResult::SUCCESS; 58 | } 59 | 60 | bool WrapperRecoveryBehavior::cancel() 61 | { 62 | return false; 63 | } 64 | 65 | WrapperRecoveryBehavior::WrapperRecoveryBehavior(boost::shared_ptr plugin) 66 | : nav_core_plugin_(plugin) 67 | {} 68 | 69 | WrapperRecoveryBehavior::~WrapperRecoveryBehavior() 70 | {} 71 | 72 | }; /* namespace mbf_abstract_core */ 73 | -------------------------------------------------------------------------------- /mbf_costmap_nav/test/config.yaml: -------------------------------------------------------------------------------- 1 | search: 2 | global: 3 | track_unknown_space: false 4 | footprint: [[-0.6, -0.6], [1.6, -0.6], [1.6, 0.6], [-0.6, 0.6]] # in Y it is a bit larger than one cell 5 | plugins: 6 | - {name: static, type: "costmap_2d::StaticLayer"} 7 | - {name: obstacles, type: "costmap_2d::ObstacleLayer"} 8 | -------------------------------------------------------------------------------- /mbf_costmap_nav/test/free_pose_search.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mbf_costmap_nav/test/record_bag_file.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Record topics required to debug tests 4 | 5 | OUTPUT_PATH="$(echo $1 | sed 's![^/]$!&/!')" 6 | echo "Bag file will be saved in "$OUTPUT_PATH 7 | 8 | rosbag record -o "$OUTPUT_PATH"mbf_test \ 9 | -e "(.*)/search_markers(.*)" \ 10 | -e "(.*)/map(.*)" \ 11 | -------------------------------------------------------------------------------- /mbf_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2021-10-26) 6 | ------------------ 7 | * return empty footprint if partly outside of the map, see #272 8 | 9 | 0.3.4 (2020-12-02) 10 | ------------------ 11 | 12 | 0.3.3 (2020-11-05) 13 | ------------------ 14 | 15 | 0.3.2 (2020-05-25) 16 | ------------------ 17 | * add impassable outcome code for recovery behaviors 18 | * enable different goal tolerance values for each action 19 | 20 | 0.3.1 (2020-04-07) 21 | ------------------ 22 | 23 | 0.3.0 (2020-03-31) 24 | ------------------ 25 | * add some more error codes, e.g. out of map, or map error 26 | * unify license declaration to BSD-3 27 | 28 | 0.2.5 (2019-10-11) 29 | ------------------ 30 | 31 | 0.2.4 (2019-06-16) 32 | ------------------ 33 | * Add check_point_cost service 34 | * Change current_twist for last_cmd_vel on exe_path/feedback 35 | 36 | 0.2.3 (2018-11-14) 37 | ------------------ 38 | * Add outcome and message to the action's feedback in ExePath and MoveBase 39 | 40 | 0.2.1 (2018-10-03) 41 | ------------------ 42 | 43 | 0.2.0 (2018-09-11) 44 | ------------------ 45 | * Concurrency for planners, controllers and recovery behaviors 46 | * Adds concurrency slots to actions 47 | 48 | 0.1.0 (2018-03-22) 49 | ------------------ 50 | * First release of move_base_flex for kinetic and lunar 51 | ca -------------------------------------------------------------------------------- /mbf_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mbf_msgs) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | actionlib_msgs 7 | genmsg 8 | geometry_msgs 9 | message_generation 10 | message_runtime 11 | nav_msgs 12 | std_msgs 13 | ) 14 | 15 | add_service_files( 16 | DIRECTORY 17 | srv 18 | FILES 19 | CheckPoint.srv 20 | CheckPose.srv 21 | CheckPath.srv 22 | FindValidPose.srv 23 | ) 24 | 25 | add_action_files( 26 | DIRECTORY 27 | action 28 | FILES 29 | GetPath.action 30 | ExePath.action 31 | Recovery.action 32 | MoveBase.action 33 | ) 34 | 35 | generate_messages( 36 | DEPENDENCIES 37 | actionlib_msgs 38 | geometry_msgs 39 | nav_msgs 40 | std_msgs 41 | ) 42 | 43 | catkin_package( 44 | CATKIN_DEPENDS 45 | actionlib_msgs 46 | geometry_msgs 47 | message_runtime 48 | nav_msgs 49 | std_msgs 50 | ) 51 | -------------------------------------------------------------------------------- /mbf_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Move Base Flex Messages, Services and Actions {#mainpage} 2 | 3 | This move_base_flex messages package provides the action definition files for the actions GetPath, ExePath, Recovery and MoveBase. The action servers providing these actions are implemented in [mbf_abstract_nav](wiki.ros.org/mbf_abstract_nav). -------------------------------------------------------------------------------- /mbf_msgs/action/ExePath.action: -------------------------------------------------------------------------------- 1 | # Follow the given path until completion or failure 2 | 3 | nav_msgs/Path path 4 | 5 | # Controller to use; defaults to the first one specified on "controllers" parameter 6 | string controller 7 | 8 | # use different slots for concurrency 9 | uint8 concurrency_slot 10 | 11 | # define goal tolerance for the action 12 | bool tolerance_from_action 13 | float32 dist_tolerance 14 | float32 angle_tolerance 15 | --- 16 | 17 | # Predefined success codes: 18 | uint8 SUCCESS = 0 19 | # 1..9 are reserved as plugin specific non-error results 20 | 21 | # Predefined error codes: 22 | uint8 FAILURE = 100 # Unspecified failure, only used for old, non-mfb_core based plugins 23 | uint8 CANCELED = 101 24 | uint8 NO_VALID_CMD = 102 25 | uint8 PAT_EXCEEDED = 103 26 | uint8 COLLISION = 104 27 | uint8 OSCILLATION = 105 28 | uint8 ROBOT_STUCK = 106 # The robot is not obeying the commanded velocity 29 | uint8 MISSED_GOAL = 107 # The robot has overshot the goal and cannot reach it anymore 30 | uint8 MISSED_PATH = 108 # The robot has diverged from the path and cannot rejoin it 31 | uint8 BLOCKED_GOAL = 109 # There's an obstacle at the goal 32 | uint8 BLOCKED_PATH = 110 # There's an obstacle on the path 33 | uint8 INVALID_PATH = 111 34 | uint8 TF_ERROR = 112 35 | uint8 NOT_INITIALIZED = 113 36 | uint8 INVALID_PLUGIN = 114 37 | uint8 INTERNAL_ERROR = 115 38 | uint8 OUT_OF_MAP = 116 # The start and / or the goal are outside the map 39 | uint8 MAP_ERROR = 117 # The map is not available or not running properly 40 | uint8 STOPPED = 118 # The controller execution has been stopped rigorously 41 | 42 | uint8 ERROR_RANGE_START = 100 43 | uint8 ERROR_RANGE_END = 149 44 | 45 | # 121..149 are reserved as plugin specific errors: 46 | uint8 PLUGIN_ERROR_RANGE_START = 121 47 | uint8 PLUGIN_ERROR_RANGE_END = 149 48 | 49 | uint32 outcome 50 | string message 51 | 52 | geometry_msgs/PoseStamped final_pose 53 | float32 dist_to_goal 54 | float32 angle_to_goal 55 | 56 | --- 57 | 58 | # Outcome of most recent controller cycle. Same values as in result 59 | uint32 outcome 60 | string message 61 | 62 | float32 dist_to_goal 63 | float32 angle_to_goal 64 | geometry_msgs/PoseStamped current_pose 65 | geometry_msgs/TwistStamped last_cmd_vel # last command calculated by the controller 66 | -------------------------------------------------------------------------------- /mbf_msgs/action/GetPath.action: -------------------------------------------------------------------------------- 1 | # Get a path from start_pose or current position to the target pose 2 | 3 | # Use start_pose or current position as the beginning of the path 4 | bool use_start_pose 5 | 6 | # The start pose for the path; optional, used if use_start_pose is true 7 | geometry_msgs/PoseStamped start_pose 8 | 9 | # The pose to achieve with the path 10 | geometry_msgs/PoseStamped target_pose 11 | 12 | # If the goal is obstructed, how many meters the planner can relax the constraint in x and y before failing 13 | float64 tolerance 14 | 15 | # Planner to use; defaults to the first one specified on "planners" parameter 16 | string planner 17 | 18 | # use different slots for concurrency 19 | uint8 concurrency_slot 20 | 21 | --- 22 | 23 | # Predefined success codes: 24 | uint8 SUCCESS = 0 25 | # 1..9 are reserved as plugin specific non-error results 26 | 27 | # Possible error codes: 28 | uint8 FAILURE = 50 # Unspecified failure, only used for old, non-mfb_core based plugins 29 | uint8 CANCELED = 51 # The action has been canceled by a action client 30 | uint8 INVALID_START = 52 # The start pose is inconsistent (e.g. frame is not valid) 31 | uint8 INVALID_GOAL = 53 # The goal pose is inconsistent (e.g. frame is not valid) 32 | uint8 BLOCKED_START = 54 # The start pose is in collision 33 | uint8 BLOCKED_GOAL = 55 # The goal pose is in collision 34 | uint8 NO_PATH_FOUND = 56 35 | uint8 PAT_EXCEEDED = 57 36 | uint8 EMPTY_PATH = 58 37 | uint8 TF_ERROR = 59 38 | uint8 NOT_INITIALIZED = 60 39 | uint8 INVALID_PLUGIN = 61 40 | uint8 INTERNAL_ERROR = 62 41 | uint8 OUT_OF_MAP = 63 # The start and / or the goal are outside the map 42 | uint8 MAP_ERROR = 64 # The map is not available or not running properly 43 | uint8 STOPPED = 65 # The planner execution has been stopped rigorously 44 | 45 | uint8 ERROR_RANGE_START = 50 46 | uint8 ERROR_RANGE_END = 99 47 | 48 | # 71..99 are reserved as plugin specific errors: 49 | uint8 PLUGIN_ERROR_RANGE_START = 71 50 | uint8 PLUGIN_ERROR_RANGE_END = 99 51 | 52 | uint32 outcome 53 | string message 54 | 55 | nav_msgs/Path path 56 | 57 | float64 cost 58 | 59 | --- 60 | -------------------------------------------------------------------------------- /mbf_msgs/action/MoveBase.action: -------------------------------------------------------------------------------- 1 | # Extension of move_base_msgs/MoveBase action, with more detailed result 2 | # and feedback and the possibility to specify lists of applicable plugins 3 | 4 | geometry_msgs/PoseStamped target_pose 5 | 6 | # Controller to use; defaults to the first one specified on "controllers" parameter 7 | string controller 8 | 9 | # Planner to use; defaults to the first one specified on "planners" parameter 10 | string planner 11 | 12 | # Recovery behaviors to try on case of failure; defaults to the "recovery_behaviors" parameter value 13 | string[] recovery_behaviors 14 | 15 | --- 16 | 17 | # Predefined success codes: 18 | uint8 SUCCESS = 0 19 | 20 | # Predefined general error codes: 21 | uint8 FAILURE = 10 22 | uint8 CANCELED = 11 23 | uint8 COLLISION = 12 24 | uint8 OSCILLATION = 13 25 | uint8 START_BLOCKED = 14 26 | uint8 GOAL_BLOCKED = 15 27 | uint8 TF_ERROR = 16 28 | uint8 INTERNAL_ERROR = 17 29 | 30 | uint8 ERROR_RANGE_START = 10 31 | uint8 ERROR_RANGE_END = 49 32 | # 21..49 are reserved for future general error codes 33 | 34 | # Planning/controlling failures: 35 | uint8 PLAN_FAILURE = 50 36 | # 51..99 are reserved as planner specific errors 37 | 38 | uint8 CTRL_FAILURE = 100 39 | # 101..149 are reserved as controller specific errors 40 | 41 | uint32 outcome 42 | string message 43 | 44 | # Configuration upon action completion 45 | float32 dist_to_goal 46 | float32 angle_to_goal 47 | geometry_msgs/PoseStamped final_pose 48 | 49 | --- 50 | 51 | # Outcome of most recent controller cycle. Same values as in MoveBase or ExePath result 52 | uint32 outcome 53 | string message 54 | 55 | float32 dist_to_goal 56 | float32 angle_to_goal 57 | geometry_msgs/PoseStamped current_pose 58 | geometry_msgs/TwistStamped last_cmd_vel # last command calculated by the controller 59 | -------------------------------------------------------------------------------- /mbf_msgs/action/Recovery.action: -------------------------------------------------------------------------------- 1 | # Run one of the recovery behavior listed on recovery_behaviors parameter 2 | 3 | string behavior 4 | 5 | # use different slots for concurrency 6 | uint8 concurrency_slot 7 | 8 | --- 9 | 10 | # Predefined success codes: 11 | uint8 SUCCESS = 0 12 | 13 | # Possible server codes: 14 | uint8 FAILURE = 150 15 | uint8 CANCELED = 151 16 | uint8 PAT_EXCEEDED = 152 17 | uint8 TF_ERROR = 153 18 | uint8 NOT_INITIALIZED = 154 19 | uint8 INVALID_PLUGIN = 155 20 | uint8 INTERNAL_ERROR = 156 21 | uint8 STOPPED = 157 # The recovery behaviour execution has been stopped rigorously 22 | uint8 IMPASSABLE = 158 # Further execution would lead to a collision 23 | 24 | uint8 ERROR_RANGE_START = 150 25 | uint8 ERROR_RANGE_END = 199 26 | 27 | # 171..199 are reserved as plugin specific errors: 28 | uint8 PLUGIN_ERROR_RANGE_START = 171 29 | uint8 PLUGIN_ERROR_RANGE_END = 199 30 | 31 | uint32 outcome 32 | string message 33 | string used_plugin 34 | 35 | --- 36 | -------------------------------------------------------------------------------- /mbf_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mbf_msgs 3 | 0.4.0 4 | 5 | The move_base_flex messages package providing the action definition files for the action GetPath, ExePath, Recovery and MoveBase. The action servers providing these action are implemented in mbf_abstract_nav. 6 | 7 | Jorge Santos 8 | Sebastian Pütz 9 | Jorge Santos 10 | Sebastian Pütz 11 | BSD-3 12 | 13 | catkin 14 | 15 | std_msgs 16 | nav_msgs 17 | geometry_msgs 18 | actionlib_msgs 19 | genmsg 20 | message_runtime 21 | message_generation 22 | 23 | std_msgs 24 | nav_msgs 25 | geometry_msgs 26 | actionlib_msgs 27 | message_runtime 28 | 29 | 30 | -------------------------------------------------------------------------------- /mbf_msgs/srv/CheckPath.srv: -------------------------------------------------------------------------------- 1 | uint8 LOCAL_COSTMAP = 1 2 | uint8 GLOBAL_COSTMAP = 2 3 | 4 | nav_msgs/Path path # the path to be checked after transforming to costmap frame 5 | float32 safety_dist # minimum distance allowed to the closest obstacle (footprint padding) 6 | float32 lethal_cost_mult # cost multiplier for cells marked as lethal obstacle (zero is ignored) 7 | float32 inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored) 8 | float32 unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored) 9 | uint8 costmap # costmap in which to check the pose 10 | uint8 return_on # abort check on finding a pose with this state or worse (zero is ignored) 11 | uint8 skip_poses # skip this number of poses between checks, to speedup processing 12 | bool use_padded_fp # include footprint padding when checking cost; note that safety distance 13 | # will be measured from the padded footprint 14 | bool path_cells_only # check only cells directly traversed by the path, ignoring robot footprint 15 | # (if true, both safety_dist and use_padded_fp are ignored) 16 | --- 17 | uint8 FREE = 0 # path is completely in traversable space 18 | uint8 INSCRIBED = 1 # path is partially in inscribed space 19 | uint8 LETHAL = 2 # path is partially in collision 20 | uint8 UNKNOWN = 3 # path is partially in unknown space 21 | uint8 OUTSIDE = 4 # path is partially outside the map 22 | 23 | uint32 last_checked # index of the first pose along the path with return_on state or worse 24 | uint8 state # path worst state (until last_checked): FREE, INSCRIBED, LETHAL, UNKNOWN... 25 | uint32 cost # cost of all cells traversed by path within footprint padded by safety_dist 26 | # or just by the path if path_cells_only is true 27 | -------------------------------------------------------------------------------- /mbf_msgs/srv/CheckPoint.srv: -------------------------------------------------------------------------------- 1 | uint8 LOCAL_COSTMAP = 1 2 | uint8 GLOBAL_COSTMAP = 2 3 | 4 | geometry_msgs/PointStamped point # the point to be checked after transforming to costmap frame 5 | uint8 costmap # costmap in which to check the point 6 | --- 7 | uint8 FREE = 0 # point is in traversable space 8 | uint8 INSCRIBED = 1 # point is in inscribed space 9 | uint8 LETHAL = 2 # point is in collision 10 | uint8 UNKNOWN = 3 # point is in unknown space 11 | uint8 OUTSIDE = 4 # point is outside the map 12 | 13 | uint8 state # point state: FREE, INSCRIBED, LETHAL, UNKNOWN or OUTSIDE 14 | uint32 cost # cost of the cell at point 15 | -------------------------------------------------------------------------------- /mbf_msgs/srv/CheckPose.srv: -------------------------------------------------------------------------------- 1 | uint8 LOCAL_COSTMAP = 1 2 | uint8 GLOBAL_COSTMAP = 2 3 | 4 | geometry_msgs/PoseStamped pose # the pose to be checked after transforming to costmap frame 5 | float32 safety_dist # minimum distance allowed to the closest obstacle 6 | float32 lethal_cost_mult # cost multiplier for cells marked as lethal obstacle (zero is ignored) 7 | float32 inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored) 8 | float32 unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored) 9 | uint8 costmap # costmap in which to check the pose 10 | bool current_pose # check current robot pose instead (ignores pose field) 11 | bool use_padded_fp # include footprint padding when checking cost; note that safety distance 12 | # will be measured from the padded footprint 13 | --- 14 | uint8 FREE = 0 # robot is completely in traversable space 15 | uint8 INSCRIBED = 1 # robot is partially in inscribed space 16 | uint8 LETHAL = 2 # robot is partially in collision 17 | uint8 UNKNOWN = 3 # robot is partially in unknown space 18 | uint8 OUTSIDE = 4 # robot is partially outside the map 19 | 20 | uint8 state # pose state: FREE, INSCRIBED, LETHAL, UNKNOWN or OUTSIDE 21 | uint32 cost # total cost of all cells within footprint padded by safety_dist 22 | -------------------------------------------------------------------------------- /mbf_msgs/srv/FindValidPose.srv: -------------------------------------------------------------------------------- 1 | # Find the closest free pose to the given one, within the given linear and angular tolerances. 2 | # 3 | # A pose is considered free if the robot footprint there is entirely inside the map and neither in 4 | # collision nor unknown space. 5 | # 6 | # If no free pose can be found, but we find one partially in unknown space, or partially outside the map, 7 | # we will return it and set state to the corresponding option (unknown space takes precedence). 8 | # Otherwise state will be set to LETHAL. 9 | # 10 | # You can also instruct this service to use current robot's pose, instead of providing one. 11 | 12 | uint8 LOCAL_COSTMAP = 1 13 | uint8 GLOBAL_COSTMAP = 2 14 | 15 | geometry_msgs/PoseStamped pose # the starting pose from which we start the search 16 | float32 safety_dist # minimum distance allowed to the closest obstacle 17 | float32 dist_tolerance # maximum distance we can deviate from the given pose during the search 18 | float32 angle_tolerance # maximum angle we can rotate the given pose during the search 19 | uint8 costmap # costmap in which to check the pose 20 | bool current_pose # check current robot pose instead (ignores pose field) 21 | bool use_padded_fp # include footprint padding when checking cost; note that safety distance 22 | # will be measured from the padded footprint 23 | --- 24 | uint8 FREE = 0 # found pose is completely in traversable space 25 | uint8 INSCRIBED = 1 # found pose is partially in inscribed space 26 | uint8 LETHAL = 2 # found pose is partially in collision 27 | uint8 UNKNOWN = 3 # found pose is partially in unknown space 28 | uint8 OUTSIDE = 4 # found pose is partially outside the map 29 | 30 | uint8 state # found pose's state: FREE, INSCRIBED, LETHAL, UNKNOWN or OUTSIDE 31 | uint32 cost # found pose's cost (sum of costs over all cells covered by the footprint) 32 | geometry_msgs/PoseStamped pose # the pose found (filled only if state is not set to LETHAL) 33 | -------------------------------------------------------------------------------- /mbf_simple_nav/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_simple_nav 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2021-10-26) 6 | ------------------ 7 | 8 | 0.3.4 (2020-12-02) 9 | ------------------ 10 | 11 | 0.3.3 (2020-11-05) 12 | ------------------ 13 | 14 | 0.3.2 (2020-05-25) 15 | ------------------ 16 | 17 | 0.3.1 (2020-04-07) 18 | ------------------ 19 | 20 | 0.3.0 (2020-03-31) 21 | ------------------ 22 | * unify license declaration to BSD-3 23 | 24 | 0.2.5 (2019-10-11) 25 | ------------------ 26 | 27 | 0.2.4 (2019-06-16) 28 | ------------------ 29 | 30 | 0.2.3 (2018-11-14) 31 | ------------------ 32 | 33 | 0.2.2 (2018-10-10) 34 | ------------------ 35 | 36 | 0.2.1 (2018-10-03) 37 | ------------------ 38 | * Make MBF melodic and indigo compatible 39 | * Fix GoalHandle references bug in callbacks 40 | 41 | 0.2.0 (2018-09-11) 42 | ------------------ 43 | * Update copyright and 3-clause-BSD license 44 | * Concurrency for planners, controllers and recovery behaviors 45 | 46 | 0.1.0 (2018-03-22) 47 | ------------------ 48 | * First release of move_base_flex for kinetic and lunar 49 | -------------------------------------------------------------------------------- /mbf_simple_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mbf_simple_nav) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | actionlib 7 | actionlib_msgs 8 | dynamic_reconfigure 9 | geometry_msgs 10 | mbf_abstract_nav 11 | mbf_msgs 12 | mbf_abstract_core 13 | nav_msgs 14 | pluginlib 15 | roscpp 16 | std_msgs 17 | std_srvs 18 | tf 19 | tf2 20 | tf2_ros 21 | ) 22 | 23 | find_package(Boost COMPONENTS thread chrono REQUIRED) 24 | 25 | set(MBF_SIMPLE_SERVER_LIB mbf_simple_server) 26 | set(MBF_SIMPLE_SERVER_NODE mbf_simple_nav) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | LIBRARIES ${MBF_SIMPLE_SERVER_LIB} 31 | CATKIN_DEPENDS 32 | actionlib 33 | actionlib_msgs 34 | dynamic_reconfigure 35 | geometry_msgs 36 | mbf_abstract_nav 37 | mbf_msgs 38 | mbf_abstract_core 39 | nav_msgs 40 | pluginlib 41 | roscpp 42 | std_msgs 43 | std_srvs 44 | tf 45 | tf2 46 | tf2_ros 47 | DEPENDS Boost 48 | ) 49 | 50 | include_directories( 51 | include 52 | ${catkin_INCLUDE_DIRS} 53 | ${Boost_INCLUDE_DIRS} 54 | ) 55 | 56 | add_library(${MBF_SIMPLE_SERVER_LIB} 57 | src/simple_navigation_server.cpp 58 | ) 59 | 60 | add_dependencies(${MBF_SIMPLE_SERVER_LIB} ${catkin_EXPORTED_TARGETS}) 61 | target_link_libraries(${MBF_SIMPLE_SERVER_LIB} 62 | ${catkin_LIBRARIES} 63 | ${Boost_LIBRARIES} 64 | ) 65 | 66 | add_executable(${MBF_SIMPLE_SERVER_NODE} src/simple_server_node.cpp) 67 | add_dependencies(${MBF_SIMPLE_SERVER_NODE} ${MBF_SIMPLE_SERVER_LIB}) 68 | target_link_libraries(${MBF_SIMPLE_SERVER_NODE} 69 | ${MBF_SIMPLE_SERVER_LIB} 70 | ${catkin_LIBRARIES}) 71 | 72 | install(TARGETS 73 | ${MBF_SIMPLE_SERVER_LIB} ${MBF_SIMPLE_SERVER_NODE} 74 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 77 | ) 78 | 79 | install(DIRECTORY include/${PROJECT_NAME}/ 80 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 81 | ) 82 | 83 | -------------------------------------------------------------------------------- /mbf_simple_nav/README.md: -------------------------------------------------------------------------------- 1 | # Move Base Flex Simple Navigation Server {#mainpage} 2 | 3 | 4 | The mbf_simple_nav package contains a simple navigation server implementation of Move Base Flex (MBF). The simple navigation server is bound to no map representation. It provides actions for planning, controlling and recovering. MBF loads all defined plugins which are defined in the lists *planners*, *controllers* and *recovery_behaviors*. Each list holds a pair of a *name* and a *type*. The *type* defines which kind of plugin to load. The *name* defines under which name the plugin should be callable by the actions. 5 | 6 | It tries to load the defined plugins which implements the defined interfaces in [mbf_abstract_core](wiki.ros.org/mbf_abstract_core). -------------------------------------------------------------------------------- /mbf_simple_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mbf_simple_nav 3 | 0.4.0 4 | 5 | The mbf_simple_nav package contains a simple navigation server implementation of Move Base Flex (MBF). The simple navigation server is bound to no map representation. It provides actions for planning, controlling and recovering. MBF loads all defined plugins which are defined in the lists *planners*, *controllers* and *recovery_behaviors*. Each list holds a pair of a *name* and a *type*. The *type* defines which kind of plugin to load. The *name* defines under which name the plugin should be callable by the actions. 6 | 7 | It tries to load the defined plugins which implements the defined interfaces in mbf_abstract_core. 8 | 9 | http://wiki.ros.org/move_base_flex 10 | Sebastian Pütz 11 | Sebastian Pütz 12 | Jorge Santos 13 | BSD-3 14 | 15 | catkin 16 | 17 | tf 18 | roscpp 19 | pluginlib 20 | actionlib 21 | actionlib_msgs 22 | dynamic_reconfigure 23 | std_msgs 24 | std_srvs 25 | nav_msgs 26 | geometry_msgs 27 | mbf_abstract_nav 28 | mbf_abstract_core 29 | mbf_msgs 30 | 31 | tf2 32 | tf2_ros 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mbf_simple_nav/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: Move Base Flex 3 | homepage: http://wiki.ros.org/move_base_flex 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.py *.md' 5 | use_mdfile_as_mainpage: README.md 6 | -------------------------------------------------------------------------------- /mbf_simple_nav/src/simple_server_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * simple_server_node.cpp 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #include "mbf_simple_nav/simple_navigation_server.h" 42 | #include 43 | #include 44 | 45 | int main(int argc, char **argv) 46 | { 47 | ros::init(argc, argv, "mbf_simple_server"); 48 | 49 | typedef boost::shared_ptr SimpleNavigationServerPtr; 50 | 51 | ros::NodeHandle nh; 52 | ros::NodeHandle private_nh("~"); 53 | 54 | double cache_time; 55 | private_nh.param("tf_cache_time", cache_time, 10.0); 56 | 57 | #ifdef USE_OLD_TF 58 | TFPtr tf_listener_ptr(new TF(nh, ros::Duration(cache_time), true)); 59 | #else 60 | TFPtr tf_listener_ptr(new TF(ros::Duration(cache_time))); 61 | tf2_ros::TransformListener tf_listener(*tf_listener_ptr); 62 | #endif 63 | 64 | SimpleNavigationServerPtr controller_ptr( 65 | new mbf_simple_nav::SimpleNavigationServer(tf_listener_ptr)); 66 | 67 | ros::spin(); 68 | return EXIT_SUCCESS; 69 | } 70 | -------------------------------------------------------------------------------- /mbf_utility/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_utility 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2021-10-26) 6 | ------------------ 7 | * return empty footprint if partly outside of the map, see #272 8 | 9 | 0.3.4 (2020-12-02) 10 | ------------------ 11 | 12 | 0.3.3 (2020-11-05) 13 | ------------------ 14 | * fix controller fails if robot pose gets older than tf_timeout 15 | * Make reference symbol position consistent across the project 16 | * Move RobotInformation to mbf_utility, as it can be used generally 17 | 18 | 0.3.2 (2020-05-25) 19 | ------------------ 20 | * Remove dependency on base_local_planner and move FootprintHelper class to mbf_costmap_nav and make it static 21 | 22 | 0.3.1 (2020-04-07) 23 | ------------------ 24 | 25 | 0.3.0 (2020-03-31) 26 | ------------------ 27 | * Add exception classes for get_path, exe_path and recovery 28 | * unify license declaration to BSD-3 29 | 30 | 0.2.5 (2019-10-11) 31 | ------------------ 32 | 33 | 0.2.4 (2019-06-16) 34 | ------------------ 35 | * Add check_point_cost service 36 | 37 | 0.2.3 (2018-11-14) 38 | ------------------ 39 | * Fix getRobotPose in melodic 40 | 41 | 0.2.2 (2018-10-10) 42 | ------------------ 43 | 44 | 0.2.1 (2018-10-03) 45 | ------------------ 46 | * Make MBF melodic and indigo compatible 47 | * Fix GoalHandle references bug in callbacks 48 | 49 | 0.2.0 (2018-09-11) 50 | ------------------ 51 | * Update copyright and 3-clause-BSD license 52 | 53 | 0.1.0 (2018-03-22) 54 | ------------------ 55 | * First release of move_base_flex for kinetic and lunar 56 | -------------------------------------------------------------------------------- /mbf_utility/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mbf_utility) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | mbf_msgs 7 | roscpp 8 | tf 9 | tf2 10 | tf2_ros 11 | tf2_geometry_msgs 12 | ) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS include 16 | LIBRARIES mbf_utility 17 | CATKIN_DEPENDS geometry_msgs mbf_msgs roscpp tf tf2 tf2_ros tf2_geometry_msgs 18 | ) 19 | 20 | include_directories( 21 | include 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | add_library(${PROJECT_NAME} 26 | src/navigation_utility.cpp 27 | src/robot_information.cpp 28 | src/odometry_helper.cpp 29 | ) 30 | 31 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 32 | 33 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 34 | 35 | install(TARGETS ${PROJECT_NAME} 36 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 39 | ) 40 | 41 | install(DIRECTORY include/${PROJECT_NAME}/ 42 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 43 | FILES_MATCHING PATTERN "*.h" 44 | ) 45 | -------------------------------------------------------------------------------- /mbf_utility/include/mbf_utility/exe_path_exception.h: -------------------------------------------------------------------------------- 1 | #ifndef MBF_UTILITY__EXE_PATH_EXCEPTION_H_ 2 | #define MBF_UTILITY__EXE_PATH_EXCEPTION_H_ 3 | 4 | #include 5 | 6 | #include "mbf_utility/navigation_utility.h" 7 | 8 | namespace mbf_utility 9 | { 10 | 11 | struct ExePathException : public std::exception 12 | { 13 | ExePathException(unsigned int error_code) : outcome(error_code), message(outcome2str(error_code)){} 14 | 15 | const char* what () const throw () 16 | { 17 | return message.c_str(); 18 | } 19 | unsigned int outcome; 20 | std::string message; 21 | }; 22 | 23 | } /* namespace mbf_utility */ 24 | 25 | #endif // MBF_UTILITY__EXE_PATH_EXCEPTION_H_ 26 | -------------------------------------------------------------------------------- /mbf_utility/include/mbf_utility/get_path_exception.h: -------------------------------------------------------------------------------- 1 | #ifndef MBF_UTILITY__GET_PATH_EXCEPTION_H_ 2 | #define MBF_UTILITY__GET_PATH_EXCEPTION_H_ 3 | 4 | #include 5 | 6 | #include "mbf_utility/navigation_utility.h" 7 | 8 | namespace mbf_utility 9 | { 10 | 11 | struct GetPathException : public std::exception 12 | { 13 | GetPathException(unsigned int error_code) : outcome(error_code), message(outcome2str(error_code)){} 14 | 15 | const char* what () const throw () 16 | { 17 | return message.c_str(); 18 | } 19 | unsigned int outcome; 20 | std::string message; 21 | }; 22 | 23 | } /* namespace mbf_utility */ 24 | 25 | #endif // MBF_UTILITY__GET_PATH_EXCEPTION_H_ 26 | -------------------------------------------------------------------------------- /mbf_utility/include/mbf_utility/navigation_utility.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * navigation_utility.h 34 | * 35 | * authors: 36 | * Sebastian Pütz 37 | * Jorge Santos Simón 38 | * 39 | */ 40 | 41 | #ifndef MBF_UTILITY__NAVIGATION_UTILITY_H_ 42 | #define MBF_UTILITY__NAVIGATION_UTILITY_H_ 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include "mbf_utility/types.h" 53 | 54 | namespace mbf_utility 55 | { 56 | 57 | /** 58 | * @brief Transforms a point from one frame into another. 59 | * @param tf_listener TransformListener. 60 | * @param target_frame Target frame for the point. 61 | * @param timeout Timeout for looking up the transformation. 62 | * @param in Point to transform. 63 | * @param out Transformed point. 64 | * @return true, if the transformation succeeded. 65 | */ 66 | bool transformPoint(const TF &tf, 67 | const std::string &target_frame, 68 | const ros::Duration &timeout, 69 | const geometry_msgs::PointStamped &in, 70 | geometry_msgs::PointStamped &out); 71 | 72 | /** 73 | * @brief Transforms a pose from one frame into another. 74 | * @param tf_listener TransformListener. 75 | * @param target_frame Target frame for the pose. 76 | * @param timeout Timeout for looking up the transformation. 77 | * @param in Pose to transform. 78 | * @param out Transformed pose. 79 | * @return true, if the transformation succeeded. 80 | */ 81 | bool transformPose(const TF &tf, 82 | const std::string &target_frame, 83 | const ros::Duration &timeout, 84 | const geometry_msgs::PoseStamped &in, 85 | geometry_msgs::PoseStamped &out); 86 | 87 | /** 88 | * @brief Computes the robot pose. 89 | * @param tf_listener TransformListener. 90 | * @param robot_frame frame of the robot. 91 | * @param global_frame global frame in which the robot is located. 92 | * @param timeout Timeout for looking up the transformation. 93 | * @param robot_pose the computed rebot pose in the global frame. 94 | * @return true, if succeeded, false otherwise. 95 | */ 96 | bool getRobotPose(const TF &tf, 97 | const std::string &robot_frame, 98 | const std::string &global_frame, 99 | const ros::Duration &timeout, 100 | geometry_msgs::PoseStamped &robot_pose); 101 | /** 102 | * @brief Computes the Euclidean-distance between two poses. 103 | * @param pose1 pose 1 104 | * @param pose2 pose 2 105 | * @return Euclidean distance between pose 1 and pose 2. 106 | */ 107 | double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2); 108 | 109 | /** 110 | * @brief Computes the smallest angle between two poses. 111 | * @param pose1 pose 1 112 | * @param pose2 pose 2 113 | * @return smallest angle between pose 1 and pose 2. 114 | */ 115 | double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2); 116 | 117 | /** 118 | * @brief Get a descriptive string for each possible MBF action outcome. 119 | * @param outcome Input outcome 120 | * @return Output descriptive string 121 | */ 122 | std::string outcome2str(unsigned int outcome); 123 | 124 | } /* namespace mbf_utility */ 125 | 126 | #endif /* MBF_UTILITY__NAVIGATION_UTILITY_H_ */ 127 | -------------------------------------------------------------------------------- /mbf_utility/include/mbf_utility/odometry_helper.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: TKruse 36 | *********************************************************************/ 37 | 38 | #ifndef ODOMETRY_HELPER_H_ 39 | #define ODOMETRY_HELPER_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace mbf_utility 47 | { 48 | 49 | class OdometryHelper 50 | { 51 | public: 52 | 53 | /** @brief Constructor. 54 | * @param odom_topic The topic on which to subscribe to Odometry 55 | * messages. If the empty string is given (the default), no 56 | * subscription is done. 57 | */ 58 | OdometryHelper(const std::string& odom_topic = ""); 59 | ~OdometryHelper() {} 60 | 61 | /** 62 | * @brief Callback for receiving odometry data 63 | * @param msg An Odometry message 64 | */ 65 | void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); 66 | 67 | /** 68 | * @brief Copy over the information 69 | * @param base_odom Copied odometry msg 70 | */ 71 | void getOdom(nav_msgs::Odometry& base_odom) const; 72 | 73 | /** @brief Set the odometry topic. This overrides what was set in the constructor, if anything. 74 | * 75 | * This unsubscribes from the old topic (if any) and subscribes to the new one (if any). 76 | * 77 | * If odom_topic is the empty string, this just unsubscribes from the previous topic. 78 | */ 79 | void setOdomTopic(const std::string& odom_topic); 80 | 81 | /** @brief Return the current odometry topic. */ 82 | std::string getOdomTopic() const { return odom_topic_; } 83 | 84 | private: 85 | // odom topic 86 | std::string odom_topic_; 87 | 88 | // we listen on odometry on the odom topic 89 | ros::Subscriber odom_sub_; 90 | nav_msgs::Odometry base_odom_; 91 | mutable boost::mutex odom_mutex_; 92 | }; 93 | 94 | } /* namespace mbf_utility */ 95 | 96 | #endif /* ODOMETRY_HELPER_H_ */ -------------------------------------------------------------------------------- /mbf_utility/include/mbf_utility/recovery_exception.h: -------------------------------------------------------------------------------- 1 | #ifndef MBF_UTILITY__RECOVERY_EXCEPTION_H_ 2 | #define MBF_UTILITY__RECOVERY_EXCEPTION_H_ 3 | 4 | #include 5 | 6 | #include "mbf_utility/navigation_utility.h" 7 | 8 | namespace mbf_utility 9 | { 10 | 11 | struct RecoveryException : public std::exception 12 | { 13 | RecoveryException(unsigned int error_code) : outcome(error_code), message(outcome2str(error_code)){} 14 | 15 | const char* what () const throw () 16 | { 17 | return message.c_str(); 18 | } 19 | unsigned int outcome; 20 | std::string message; 21 | }; 22 | 23 | } /* namespace mbf_utility */ 24 | 25 | #endif // MBF_UTILITY__RECOVERY_EXCEPTION_H_ 26 | -------------------------------------------------------------------------------- /mbf_utility/include/mbf_utility/robot_information.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * robot_information.h 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #ifndef MBF_UTILITY__ROBOT_INFORMATION_H_ 40 | #define MBF_UTILITY__ROBOT_INFORMATION_H_ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "mbf_utility/odometry_helper.h" 49 | #include "mbf_utility/types.h" 50 | 51 | namespace mbf_utility 52 | { 53 | 54 | class RobotInformation 55 | { 56 | public: 57 | 58 | typedef boost::shared_ptr Ptr; 59 | 60 | RobotInformation( 61 | TF &tf_listener, 62 | const std::string &global_frame, 63 | const std::string &robot_frame, 64 | const ros::Duration &tf_timeout, 65 | const std::string &odom_topic = "odom"); 66 | 67 | /** 68 | * @brief Computes the current robot pose (robot_frame_) in the global frame (global_frame_). 69 | * @param robot_pose Reference to the robot_pose message object to be filled. 70 | * @return true, if the current robot pose could be computed, false otherwise. 71 | */ 72 | bool getRobotPose(geometry_msgs::PoseStamped &robot_pose) const; 73 | 74 | /** 75 | * @brief Returns the current robot velocity, as provided by the odometry helper. 76 | * @param robot_velocity Reference to the robot_velocity message object to be filled. 77 | * @return true, if the current robot velocity could be obtained, false otherwise. 78 | */ 79 | bool getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity) const; 80 | 81 | /** 82 | * @brief Check whether the robot is stopped or not 83 | * @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped 84 | * @param trans_stopped_velocity The translational velocity below which the robot is considered stopped 85 | * @return true if the robot is stopped, false otherwise 86 | */ 87 | bool isRobotStopped(double rot_stopped_velocity, double trans_stopped_velocity) const; 88 | 89 | const std::string& getGlobalFrame() const; 90 | 91 | const std::string& getRobotFrame() const; 92 | 93 | const TF& getTransformListener() const; 94 | 95 | const ros::Duration& getTfTimeout() const; 96 | 97 | private: 98 | const TF& tf_listener_; 99 | 100 | const std::string &global_frame_; 101 | 102 | const std::string &robot_frame_; 103 | 104 | const ros::Duration &tf_timeout_; 105 | 106 | OdometryHelper odom_helper_; 107 | 108 | }; 109 | 110 | } /* mbf_utility */ 111 | 112 | #endif /* MBF_UTILITY__ROBOT_INFORMATION_H_ */ 113 | -------------------------------------------------------------------------------- /mbf_utility/include/mbf_utility/types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * abstract_planner.h 34 | * 35 | * author: Jannik Abbenseth 36 | * 37 | */ 38 | 39 | #ifndef MBF_UTILITY__TYPES_H_ 40 | #define MBF_UTILITY__TYPES_H_ 41 | 42 | #include 43 | #include 44 | 45 | #if ROS_VERSION_MINIMUM (1, 14, 0) // if current ros version is >= 1.14.0 46 | // Melodic uses TF2 47 | #include 48 | typedef boost::shared_ptr TFPtr; 49 | typedef tf2_ros::Buffer TF; 50 | typedef tf2::TransformException TFException; 51 | #else 52 | // Previous versions still using TF 53 | #define USE_OLD_TF 54 | #include 55 | typedef boost::shared_ptr TFPtr; 56 | typedef tf::TransformListener TF; 57 | typedef tf::TransformException TFException; 58 | #endif 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /mbf_utility/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mbf_utility 4 | 0.4.0 5 | The mbf_utility package 6 | 7 | Sebastian Pütz 8 | 9 | BSD-3 10 | http://wiki.ros.org/move_base_flex/mbf_utility 11 | Sebastian Pütz 12 | 13 | catkin 14 | 15 | geometry_msgs 16 | mbf_msgs 17 | roscpp 18 | tf 19 | tf2 20 | tf2_ros 21 | tf2_geometry_msgs 22 | 23 | -------------------------------------------------------------------------------- /mbf_utility/src/odometry_helper.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: TKruse 36 | *********************************************************************/ 37 | #include 38 | 39 | namespace mbf_utility 40 | { 41 | 42 | OdometryHelper::OdometryHelper(const std::string& odom_topic) 43 | { 44 | setOdomTopic(odom_topic); 45 | } 46 | 47 | void OdometryHelper::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) 48 | { 49 | ROS_INFO_STREAM_ONCE("Odometry received on topic " << getOdomTopic()); 50 | 51 | // we assume that the odometry is published in the frame of the base 52 | boost::mutex::scoped_lock lock(odom_mutex_); 53 | base_odom_ = *msg; 54 | if (base_odom_.header.stamp.isZero()) 55 | base_odom_.header.stamp = ros::Time::now(); 56 | } 57 | 58 | void OdometryHelper::getOdom(nav_msgs::Odometry& base_odom) const 59 | { 60 | boost::mutex::scoped_lock lock(odom_mutex_); 61 | base_odom = base_odom_; 62 | } 63 | 64 | void OdometryHelper::setOdomTopic(const std::string& odom_topic) 65 | { 66 | if (odom_topic != odom_topic_) 67 | { 68 | odom_topic_ = odom_topic; 69 | 70 | if (!odom_topic_.empty()) 71 | { 72 | ros::NodeHandle gn; 73 | odom_sub_ = 74 | gn.subscribe(odom_topic_, 1, boost::bind(&OdometryHelper::odomCallback, this, _1)); 75 | } 76 | else 77 | { 78 | odom_sub_.shutdown(); 79 | } 80 | } 81 | } 82 | 83 | } // namespace mbf_utility 84 | -------------------------------------------------------------------------------- /mbf_utility/src/robot_information.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, Sebastian Pütz 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions 6 | * are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * robot_information.cpp 34 | * 35 | * author: Sebastian Pütz 36 | * 37 | */ 38 | 39 | #include "mbf_utility/robot_information.h" 40 | #include "mbf_utility/navigation_utility.h" 41 | 42 | namespace mbf_utility 43 | { 44 | 45 | RobotInformation::RobotInformation(TF &tf_listener, 46 | const std::string &global_frame, 47 | const std::string &robot_frame, 48 | const ros::Duration &tf_timeout, 49 | const std::string &odom_topic) 50 | : tf_listener_(tf_listener), global_frame_(global_frame), robot_frame_(robot_frame), tf_timeout_(tf_timeout), 51 | odom_helper_(odom_topic) 52 | { 53 | 54 | } 55 | 56 | 57 | bool RobotInformation::getRobotPose(geometry_msgs::PoseStamped &robot_pose) const 58 | { 59 | bool tf_success = mbf_utility::getRobotPose(tf_listener_, robot_frame_, global_frame_, 60 | ros::Duration(tf_timeout_), robot_pose); 61 | if (!tf_success) 62 | { 63 | ROS_ERROR_STREAM("Can not get the robot pose in the global frame. - robot frame: \"" 64 | << robot_frame_ << "\" global frame: \"" << global_frame_ << "\""); 65 | return false; 66 | } 67 | return true; 68 | } 69 | 70 | bool RobotInformation::getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity) const 71 | { 72 | if (odom_helper_.getOdomTopic().empty()) 73 | { 74 | ROS_DEBUG_THROTTLE(2, "Odometry topic set as empty; ignoring retrieve velocity requests"); 75 | return true; 76 | } 77 | 78 | nav_msgs::Odometry base_odom; 79 | odom_helper_.getOdom(base_odom); 80 | if (base_odom.header.stamp.isZero()) 81 | { 82 | ROS_WARN_STREAM_THROTTLE(2, "No messages received on topic " << odom_helper_.getOdomTopic() 83 | << "; robot velocity unknown"); 84 | ROS_WARN_STREAM_THROTTLE(2, "You can disable these warnings by setting parameter 'odom_topic' as empty"); 85 | return false; 86 | } 87 | robot_velocity.header = base_odom.header; 88 | robot_velocity.twist = base_odom.twist.twist; 89 | return true; 90 | } 91 | 92 | bool RobotInformation::isRobotStopped(double rot_stopped_velocity, double trans_stopped_velocity) const 93 | { 94 | nav_msgs::Odometry base_odom; 95 | odom_helper_.getOdom(base_odom); 96 | return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity && 97 | fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity && 98 | fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity; 99 | } 100 | 101 | const std::string& RobotInformation::getGlobalFrame() const {return global_frame_;}; 102 | 103 | const std::string& RobotInformation::getRobotFrame() const {return robot_frame_;}; 104 | 105 | const TF& RobotInformation::getTransformListener() const {return tf_listener_;}; 106 | 107 | const ros::Duration& RobotInformation::getTfTimeout() const {return tf_timeout_;} 108 | 109 | } /* namespace mbf_utility */ 110 | -------------------------------------------------------------------------------- /move_base_flex/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package move_base_flex 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.0 (2021-10-26) 6 | ------------------ 7 | 8 | 0.3.4 (2020-12-02) 9 | ------------------ 10 | 11 | 0.3.3 (2020-11-05) 12 | ------------------ 13 | * Add mbf_utility to move_base_flex metapackag 14 | 15 | 0.3.2 (2020-05-25) 16 | ------------------ 17 | 18 | 0.3.1 (2020-04-07) 19 | ------------------ 20 | 21 | 0.3.0 (2020-03-31) 22 | ------------------ 23 | * unify license declaration to BSD-3 24 | 25 | 0.2.5 (2019-10-11) 26 | ------------------ 27 | 28 | 0.2.4 (2019-06-16) 29 | ------------------ 30 | 31 | 0.2.3 (2018-11-14) 32 | ------------------ 33 | 34 | 0.2.2 (2018-10-10) 35 | ------------------ 36 | 37 | 0.2.1 (2018-10-03) 38 | ------------------ 39 | 40 | 0.2.0 (2018-09-11) 41 | ------------------ 42 | * Update copyright and 3-clause-BSD license of the Move Base Flex stack 43 | * Concurrency for planners, controllers and recovery behaviors 44 | * New class structure, allowing multiple executoin instances 45 | * Fixes minor bugs 46 | 47 | 0.1.0 (2018-03-22) 48 | ------------------ 49 | * First release of move_base_flex for kinetic and lunar 50 | -------------------------------------------------------------------------------- /move_base_flex/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(move_base_flex) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /move_base_flex/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | move_base_flex 4 | 0.4.0 5 | 6 | Move Base Flex (MBF) is a backwards-compatible replacement for move_base. MBF can use existing plugins for move_base, and provides an enhanced version of the planner, controller and recovery plugin ROS interfaces. It exposes action servers for planning, controlling and recovering, providing detailed information of the current state and the plugin’s feedback. An external executive logic can use MBF and its actions to perform smart and flexible navigation strategies. Furthermore, MBF enables the use of other map representations, e.g. meshes or grid_map 7 | 8 | This package is a meta package and refers to the Move Base Flex stack packages.The abstract core of MBF – without any binding to a map representation – is represented by the mbf_abstract_nav and the mbf_abstract_core. For navigation on costmaps see mbf_costmap_nav and mbf_costmap_core. 9 | 10 | 11 | Sebastian Pütz 12 | BSD-3 13 | http://wiki.ros.org/move_base_flex 14 | Sebastian Pütz 15 | 16 | catkin 17 | mbf_abstract_core 18 | mbf_abstract_nav 19 | mbf_costmap_core 20 | mbf_costmap_nav 21 | mbf_msgs 22 | mbf_simple_nav 23 | mbf_utility 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /run-style-check-diff.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set +e 4 | 5 | # We need to add a new remote for the upstream master, since this script could 6 | # be running in a personal fork of the repository which has out of date branches. 7 | git remote add upstream https://github.com/magazino/move_base_flex 8 | git fetch upstream 9 | 10 | # on github we are somehow not on the branch itself but in detached HEAD state. 11 | # in order to run the check below we need to checkout our branch. 12 | git checkout ${GITHUB_HEAD_REF} 13 | 14 | # get the common ancestor from our branch to the base. 15 | newest_common_ancestor_sha=$(git merge-base ${GITHUB_HEAD_REF} upstream/${GITHUB_BASE_REF}) 16 | 17 | # run the format-code - and cound the lines 18 | lines_diff_count=$(./git-clang-format.py --diff ${newest_common_ancestor_sha} | wc -l) 19 | 20 | # in case of no diff we return a one-liner 21 | if [ ${lines_diff_count} -gt 1 ]; then 22 | exit 1 23 | else 24 | exit 0 25 | fi 26 | --------------------------------------------------------------------------------