├── .github └── workflows │ ├── humble.yaml │ └── jazzy.yaml ├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── cvp_mesh_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── cvp_mesh_planner.xml ├── include │ └── cvp_mesh_planner │ │ └── cvp_mesh_planner.h ├── package.xml └── src │ └── cvp_mesh_planner.cpp ├── dijkstra_mesh_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── dijkstra_mesh_planner.xml ├── include │ └── dijkstra_mesh_planner │ │ └── dijkstra_mesh_planner.h ├── package.xml └── src │ └── dijkstra_mesh_planner.cpp ├── docs └── images │ ├── costlayers │ ├── border.png │ ├── clearance.jpg │ ├── height_diff.jpg │ ├── inflation.jpg │ ├── ridge.jpg │ ├── roughness.jpg │ └── steepness.jpg │ ├── demo.gif │ ├── mesh_navigation_logo.png │ ├── roscon2023_talk.png │ └── stone_quarry │ ├── cloud.png │ ├── dem_side.jpg │ ├── dijkstra_pot.jpg │ ├── fmm_pot.jpg │ ├── height_diff.jpg │ ├── mesh_rgb.jpg │ ├── pot_fmm_vs_dijkstra.png │ └── pot_paths.png ├── mbf_mesh_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mbf_mesh_core │ │ ├── mesh_controller.h │ │ ├── mesh_planner.h │ │ └── mesh_recovery.h └── package.xml ├── mbf_mesh_nav ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── MoveBaseFlex.cfg ├── include │ └── mbf_mesh_nav │ │ ├── mesh_controller_execution.h │ │ ├── mesh_navigation_server.h │ │ ├── mesh_planner_execution.h │ │ └── mesh_recovery_execution.h ├── package.xml └── src │ ├── mbf_mesh_nav.cpp │ ├── mesh_controller_execution.cpp │ ├── mesh_navigation_server.cpp │ ├── mesh_planner_execution.cpp │ └── mesh_recovery_execution.cpp ├── mesh_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ └── MeshController.cfg ├── include │ └── mesh_controller │ │ └── mesh_controller.h ├── mesh_controller.xml ├── package.xml └── src │ └── mesh_controller.cpp ├── mesh_layers ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mesh_layers │ │ ├── border_layer.h │ │ ├── clearance_layer.h │ │ ├── height_diff_layer.h │ │ ├── inflation_layer.h │ │ ├── ridge_layer.h │ │ ├── roughness_layer.h │ │ └── steepness_layer.h ├── mesh_layers.xml ├── package.xml └── src │ ├── border_layer.cpp │ ├── clearance_layer.cpp │ ├── height_diff_layer.cpp │ ├── inflation_layer.cpp │ ├── ridge_layer.cpp │ ├── roughness_layer.cpp │ └── steepness_layer.cpp ├── mesh_map ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── mesh_map │ │ ├── abstract_layer.h │ │ ├── mesh_map.h │ │ ├── nanoflann.hpp │ │ ├── nanoflann_mesh_adaptor.h │ │ └── util.h ├── package.xml ├── src │ ├── mesh_map.cpp │ └── util.cpp └── test │ ├── layer_plugin.cpp │ ├── layer_plugin.xml │ └── mesh_map_test.cpp ├── mesh_navigation ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml └── source_dependencies.yaml /.github/workflows/humble.yaml: -------------------------------------------------------------------------------- 1 | name: Humble CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - 'main' 7 | pull_request: 8 | workflow_dispatch: 9 | branches: 10 | - '*' 11 | 12 | jobs: 13 | humble_build_and_test: 14 | uses: naturerobots/github_automation_public/.github/workflows/ros_ci.yaml@main 15 | secrets: inherit 16 | with: 17 | ros_distro: humble 18 | -------------------------------------------------------------------------------- /.github/workflows/jazzy.yaml: -------------------------------------------------------------------------------- 1 | name: Jazzy CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - 'main' 7 | pull_request: 8 | workflow_dispatch: 9 | branches: 10 | - '*' 11 | 12 | jobs: 13 | jazzy_build_and_test: 14 | uses: naturerobots/github_automation_public/.github/workflows/ros_ci.yaml@main 15 | secrets: inherit 16 | with: 17 | ros_distro: jazzy 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | # clangd stuff 54 | compile_commands.json 55 | .clangd 56 | .ccls 57 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/.gitmodules -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, Sebastian Pütz 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 | -------------------------------------------------------------------------------- /cvp_mesh_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cvp_mesh_planner 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | * rename to cvp_mesh_planner 12 | 13 | 1.0.0 (2020-12-16) 14 | ------------------ 15 | * Initial release 16 | -------------------------------------------------------------------------------- /cvp_mesh_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cvp_mesh_planner) 3 | 4 | # DEFAULT RELEASE 5 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 8 | endif() 9 | endif() 10 | 11 | find_package(ament_cmake_ros REQUIRED) 12 | find_package(mbf_mesh_core REQUIRED) 13 | find_package(mbf_msgs REQUIRED) 14 | find_package(mbf_utility REQUIRED) 15 | find_package(mesh_map REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(LVR2 REQUIRED) 18 | 19 | pluginlib_export_plugin_description_file(mbf_mesh_core cvp_mesh_planner.xml) 20 | 21 | add_library(${PROJECT_NAME} src/cvp_mesh_planner.cpp) 22 | target_include_directories(${PROJECT_NAME} PUBLIC 23 | $ 24 | $ 25 | ${LVR2_INCLUDE_DIRS}) 26 | target_compile_definitions(${PROJECT_NAME} PRIVATE "CVP_MESH_PLANNER_BUILDING_LIBRARY") 27 | ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) 28 | target_link_libraries(${PROJECT_NAME} 29 | ${LVR2_LIBRARIES} 30 | ${MPI_CXX_LIBRARIES} 31 | ) 32 | 33 | install(DIRECTORY include DESTINATION include) 34 | install(TARGETS ${PROJECT_NAME} 35 | EXPORT export_${PROJECT_NAME} 36 | ARCHIVE DESTINATION lib 37 | LIBRARY DESTINATION lib 38 | RUNTIME DESTINATION bin 39 | ) 40 | 41 | ament_export_include_directories(include) 42 | ament_export_libraries(${PROJECT_NAME}) 43 | ament_export_targets(export_${PROJECT_NAME}) 44 | ament_export_dependencies(mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) 45 | ament_package() -------------------------------------------------------------------------------- /cvp_mesh_planner/cvp_mesh_planner.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | The Continuous Vector Field Planner (CVP) for mbf_mesh_nav 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /cvp_mesh_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cvp_mesh_planner 4 | 2.0.0 5 | The Continuous Vector Field Planner (CVP) mesh planner package 6 | Matthias Holoch 7 | Sebastian Pütz 8 | BSD 3-Clause 9 | Sebastian Pütz 10 | 11 | rclcpp 12 | mbf_mesh_core 13 | mbf_utility 14 | mbf_msgs 15 | mesh_map 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /dijkstra_mesh_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package dijkstra_mesh_planner 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | 12 | 1.0.0 (2020-12-16) 13 | ------------------ 14 | * Initial release 15 | -------------------------------------------------------------------------------- /dijkstra_mesh_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(dijkstra_mesh_planner) 3 | 4 | # DEFAULT RELEASE 5 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 8 | endif() 9 | endif() 10 | 11 | find_package(ament_cmake_ros REQUIRED) 12 | find_package(mbf_mesh_core REQUIRED) 13 | find_package(mbf_msgs REQUIRED) 14 | find_package(mbf_utility REQUIRED) 15 | find_package(mesh_map REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(LVR2 REQUIRED) 18 | 19 | pluginlib_export_plugin_description_file(mbf_mesh_core dijkstra_mesh_planner.xml) 20 | 21 | add_library(${PROJECT_NAME} 22 | src/dijkstra_mesh_planner.cpp 23 | ) 24 | target_include_directories(${PROJECT_NAME} PUBLIC 25 | $ 26 | $ 27 | ${LVR2_INCLUDE_DIRS}) 28 | target_compile_definitions(${PROJECT_NAME} PRIVATE "DIJKSTRA_MESH_PLANNER_BUILDING_LIBRARY") 29 | ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) 30 | target_link_libraries(${PROJECT_NAME} 31 | ${LVR2_LIBRARIES} 32 | ${MPI_CXX_LIBRARIES} 33 | ) 34 | 35 | install(DIRECTORY include/ 36 | DESTINATION include 37 | ) 38 | install(TARGETS ${PROJECT_NAME} 39 | EXPORT export_${PROJECT_NAME} 40 | ARCHIVE DESTINATION lib 41 | LIBRARY DESTINATION lib 42 | RUNTIME DESTINATION bin 43 | ) 44 | 45 | ament_export_include_directories(include) 46 | ament_export_libraries(${PROJECT_NAME}) 47 | ament_export_targets(export_${PROJECT_NAME}) 48 | ament_export_dependencies(mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) 49 | ament_package() -------------------------------------------------------------------------------- /dijkstra_mesh_planner/dijkstra_mesh_planner.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | A Dijkstra mesh planner for mbf_mesh_nav 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MESH_NAVIGATION__dijkstra_mesh_planner_H 39 | #define MESH_NAVIGATION__DIJKSTRA_MESH_PLANNER_H 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace dijkstra_mesh_planner 47 | { 48 | 49 | class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner 50 | { 51 | public: 52 | typedef std::shared_ptr Ptr; 53 | 54 | DijkstraMeshPlanner(); 55 | 56 | /** 57 | * @brief Destructor 58 | */ 59 | virtual ~DijkstraMeshPlanner(); 60 | 61 | /** 62 | * @brief Given a goal pose in the world, compute a plan 63 | * 64 | * @param start The start pose 65 | * @param goal The goal pose 66 | * @param tolerance If the goal is obstructed, how many meters the planner can 67 | * relax the constraint in x and y before failing 68 | * @param plan The plan... filled by the planner 69 | * @param cost The cost for the the plan 70 | * @param message Optional more detailed outcome as a string 71 | * 72 | * @return Result code as described on GetPath action result: 73 | * SUCCESS = 0 74 | * 1..9 are reserved as plugin specific non-error results 75 | * FAILURE = 50 # Unspecified failure, only used for old, 76 | * non-mfb_core based plugins CANCELED = 51 INVALID_START = 52 77 | * INVALID_GOAL = 53 78 | * NO_PATH_FOUND = 54 79 | * PAT_EXCEEDED = 55 80 | * EMPTY_PATH = 56 81 | * TF_ERROR = 57 82 | * NOT_INITIALIZED = 58 83 | * INVALID_PLUGIN = 59 84 | * INTERNAL_ERROR = 60 85 | * 71..99 are reserved as plugin specific errors 86 | */ 87 | virtual uint32_t makePlan(const geometry_msgs::msg::PoseStamped& start, const geometry_msgs::msg::PoseStamped& goal, 88 | double tolerance, std::vector& plan, double& cost, 89 | std::string& message) override; 90 | 91 | /** 92 | * @brief Requests the planner to cancel, e.g. if it takes too much time. 93 | * 94 | * @return True if a cancel has been successfully requested, false if not 95 | * implemented. 96 | */ 97 | virtual bool cancel() override; 98 | 99 | /** 100 | * @brief initializes this planner with the given plugin name and map 101 | * 102 | * @param name name of this plugin 103 | * @param mesh_map_ptr environment map on which planning is done 104 | * 105 | * @return true if initialization was successul; else false 106 | */ 107 | virtual bool initialize(const std::string& plugin_name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) override; 108 | 109 | /** 110 | * @brief delivers vector field which has been generated during the latest planning 111 | * 112 | * @return vector field of the plan 113 | */ 114 | lvr2::DenseVertexMap getVectorMap(); 115 | 116 | protected: 117 | /** 118 | * @brief runs dijkstra path planning and stores the resulting distances and predecessors to the fields potential and 119 | * predecessors of this class 120 | * 121 | * @param start[in] 3D starting position of the requested path 122 | * @param goal[in] 3D goal position of the requested path 123 | * @param path[out] optimal path from the given starting position to tie goal position 124 | * 125 | * @return result code in form of GetPath action result: SUCCESS, NO_PATH_FOUND, INVALID_START, INVALID_GOAL, and 126 | * CANCELED are possible 127 | */ 128 | uint32_t dijkstra(const mesh_map::Vector& start, const mesh_map::Vector& goal, std::list& path); 129 | 130 | /** 131 | * @brief runs dijkstra path planning 132 | * 133 | * @param start[in] 3D starting position of the requested path 134 | * @param goal[in] 3D goal position of the requested path 135 | * @param edge_weights[in] edge distances of the map 136 | * @param costs[in] vertex costs of the map 137 | * @param path[out] optimal path from the given starting position to tie goal position 138 | * @param distances[out] per vertex distances to goal 139 | * @param predecessors[out] dense predecessor map for all visited vertices 140 | * 141 | * @return result code in form of GetPath action result: SUCCESS, NO_PATH_FOUND, INVALID_START, INVALID_GOAL, and 142 | * CANCELED are possible 143 | */ 144 | uint32_t dijkstra(const mesh_map::Vector& start, const mesh_map::Vector& goal, 145 | const lvr2::DenseEdgeMap& edge_weights, const lvr2::DenseVertexMap& costs, 146 | std::list& path, lvr2::DenseVertexMap& distances, 147 | lvr2::DenseVertexMap& predecessors); 148 | 149 | /** 150 | * @brief calculates the vector field based on the current predecessors map and stores it to the vector_map field of this class 151 | */ 152 | void computeVectorMap(); 153 | 154 | /** 155 | * @brief gets called whenever the node's parameters change 156 | 157 | * @param parameters vector of changed parameters. 158 | * Note that this vector will also contain parameters not related to the dijkstra mesh planner. 159 | */ 160 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 161 | 162 | private: 163 | // current map 164 | mesh_map::MeshMap::Ptr mesh_map_; 165 | // name of this plugin 166 | std::string name_; 167 | // node handle 168 | rclcpp::Node::SharedPtr node_; 169 | // true if the abort of the current planning was requested; else false 170 | std::atomic_bool cancel_planning_; 171 | // publisher of resulting path 172 | rclcpp::Publisher::SharedPtr path_pub_; 173 | // tf frame of the map 174 | std::string map_frame_; 175 | // handle of callback for changing parameters dynamically 176 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_; 177 | // config determined by ROS params; Init values defined here are used as default ROS param value 178 | struct { 179 | // publisher of resulting vector fiels 180 | bool publish_vector_field = false; 181 | // publisher of per face vectorfield 182 | bool publish_face_vectors = false; 183 | // offset of maximum distance from goal position 184 | double goal_dist_offset = 0.3; 185 | // defines the vertex cost limit with which it can be accessed 186 | double cost_limit = 1.0; 187 | } config_; 188 | 189 | // predecessors while wave propagation 190 | lvr2::DenseVertexMap predecessors_; 191 | // the face which is cut by line to the source 192 | lvr2::DenseVertexMap cutting_faces_; 193 | // stores the current vector map containing vectors pointing to the source 194 | // (path goal) 195 | lvr2::DenseVertexMap vector_map_; 196 | // potential field or distance values to the source (path goal) 197 | lvr2::DenseVertexMap potential_; 198 | }; 199 | 200 | } // namespace dijkstra_mesh_planner 201 | 202 | #endif // MESH_NAVIGATION__DIJKSTRA_MESH_PLANNER_H 203 | -------------------------------------------------------------------------------- /dijkstra_mesh_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dijkstra_mesh_planner 4 | 2.0.0 5 | The dijkstra_mesh_planner package 6 | Matthias Holoch 7 | Sebastian Pütz 8 | BSD-3 9 | Sebastian Pütz 10 | 11 | rclcpp 12 | mbf_mesh_core 13 | mbf_utility 14 | mbf_msgs 15 | mesh_map 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /docs/images/costlayers/border.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/costlayers/border.png -------------------------------------------------------------------------------- /docs/images/costlayers/clearance.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/costlayers/clearance.jpg -------------------------------------------------------------------------------- /docs/images/costlayers/height_diff.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/costlayers/height_diff.jpg -------------------------------------------------------------------------------- /docs/images/costlayers/inflation.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/costlayers/inflation.jpg -------------------------------------------------------------------------------- /docs/images/costlayers/ridge.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/costlayers/ridge.jpg -------------------------------------------------------------------------------- /docs/images/costlayers/roughness.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/costlayers/roughness.jpg -------------------------------------------------------------------------------- /docs/images/costlayers/steepness.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/costlayers/steepness.jpg -------------------------------------------------------------------------------- /docs/images/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/demo.gif -------------------------------------------------------------------------------- /docs/images/mesh_navigation_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/mesh_navigation_logo.png -------------------------------------------------------------------------------- /docs/images/roscon2023_talk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/roscon2023_talk.png -------------------------------------------------------------------------------- /docs/images/stone_quarry/cloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/cloud.png -------------------------------------------------------------------------------- /docs/images/stone_quarry/dem_side.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/dem_side.jpg -------------------------------------------------------------------------------- /docs/images/stone_quarry/dijkstra_pot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/dijkstra_pot.jpg -------------------------------------------------------------------------------- /docs/images/stone_quarry/fmm_pot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/fmm_pot.jpg -------------------------------------------------------------------------------- /docs/images/stone_quarry/height_diff.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/height_diff.jpg -------------------------------------------------------------------------------- /docs/images/stone_quarry/mesh_rgb.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/mesh_rgb.jpg -------------------------------------------------------------------------------- /docs/images/stone_quarry/pot_fmm_vs_dijkstra.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/pot_fmm_vs_dijkstra.png -------------------------------------------------------------------------------- /docs/images/stone_quarry/pot_paths.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_navigation/e80fd30138d3212dd3803c52a23a5c21d2196e61/docs/images/stone_quarry/pot_paths.png -------------------------------------------------------------------------------- /mbf_mesh_core/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_mesh_core 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | 12 | 1.0.0 (2020-12-16) 13 | ------------------ 14 | * Initial release 15 | -------------------------------------------------------------------------------- /mbf_mesh_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mbf_mesh_core) 3 | 4 | # DEFAULT RELEASE 5 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 8 | endif() 9 | endif() 10 | 11 | find_package(ament_cmake_ros REQUIRED) 12 | set(dependencies 13 | mbf_abstract_core 14 | mesh_map 15 | ) 16 | 17 | foreach(dep IN LISTS dependencies) 18 | find_package(${dep} REQUIRED) 19 | endforeach() 20 | 21 | include_directories( 22 | include 23 | ) 24 | 25 | install(DIRECTORY include/ 26 | DESTINATION include 27 | ) 28 | 29 | ament_export_include_directories(include) 30 | ament_export_dependencies(${dependencies}) 31 | ament_package() -------------------------------------------------------------------------------- /mbf_mesh_core/include/mbf_mesh_core/mesh_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * author: Sebastian Pütz 34 | * 35 | */ 36 | 37 | #ifndef MBF_MESH_CORE__MESH_CONTROLLER_H 38 | #define MBF_MESH_CORE__MESH_CONTROLLER_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | namespace mbf_mesh_core 50 | { 51 | class MeshController : public mbf_abstract_core::AbstractController 52 | { 53 | public: 54 | typedef std::shared_ptr Ptr; 55 | 56 | MeshController() {}; 57 | 58 | /** 59 | * @brief Destructor 60 | */ 61 | virtual ~MeshController() {}; 62 | 63 | /** 64 | * @brief Given the current position, orientation, and velocity of the robot, 65 | * compute velocity commands to send to the base. 66 | * @param pose The current pose of the robot. 67 | * @param velocity The current velocity of the robot. 68 | * @param cmd_vel Will be filled with the velocity command to be passed to the 69 | * robot base. 70 | * @param message Optional more detailed outcome as a string 71 | * @return Result code as described on ExePath action result (see ExePath.action) 72 | */ 73 | virtual uint32_t computeVelocityCommands(const geometry_msgs::msg::PoseStamped& pose, 74 | const geometry_msgs::msg::TwistStamped& velocity, 75 | geometry_msgs::msg::TwistStamped& cmd_vel, std::string& message) = 0; 76 | 77 | /** 78 | * @brief Check if the goal pose has been achieved by the local planner 79 | * @param angle_tolerance The angle tolerance in which the current pose will 80 | * be partly accepted as reached goal 81 | * @param dist_tolerance The distance tolerance in which the current pose will 82 | * be partly accepted as reached goal 83 | * @return True if achieved, false otherwise 84 | */ 85 | virtual bool isGoalReached(double dist_tolerance, double angle_tolerance) = 0; 86 | 87 | /** 88 | * @brief Set the plan that the local planner is following 89 | * @param plan The plan to pass to the local planner 90 | * @return True if the plan was updated successfully, false otherwise 91 | */ 92 | virtual bool setPlan(const std::vector& plan) = 0; 93 | 94 | /** 95 | * @brief Requests the planner to cancel, e.g. if it takes too much time. 96 | * @return True if a cancel has been successfully requested, false if not 97 | * implemented. 98 | */ 99 | virtual bool cancel() = 0; 100 | 101 | /** 102 | * @brief Initializes the controller plugin with a name, a tf pointer and a mesh map pointer 103 | * @param plugin_name The controller plugin name, defined by the user. It defines the controller namespace 104 | * @param tf_ptr A shared pointer to a transformation buffer 105 | * @param mesh_map_ptr A shared pointer to the mesh map 106 | * @return true if the plugin has been initialized successfully 107 | */ 108 | virtual bool initialize(const std::string& name, 109 | const std::shared_ptr& tf_ptr, 110 | const std::shared_ptr& mesh_map_ptr, 111 | const rclcpp::Node::SharedPtr& node) = 0; 112 | }; 113 | } /* namespace mbf_mesh_core */ 114 | 115 | #endif /* MBF_MESH_NAVIGATION__MESH_CONTROLLER_H */ 116 | -------------------------------------------------------------------------------- /mbf_mesh_core/include/mbf_mesh_core/mesh_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * author: Sebastian Pütz 34 | * 35 | */ 36 | 37 | #ifndef MBF_MESH_CORE__MESH_PLANNER_H 38 | #define MBF_MESH_CORE__MESH_PLANNER_H 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | namespace mbf_mesh_core 49 | { 50 | class MeshPlanner : public mbf_abstract_core::AbstractPlanner 51 | { 52 | public: 53 | typedef std::shared_ptr Ptr; 54 | 55 | /** 56 | * @brief Destructor 57 | */ 58 | virtual ~MeshPlanner() {}; 59 | 60 | /** 61 | * @brief Given a goal pose in the world, compute a plan 62 | * @param start The start pose 63 | * @param goal The goal pose 64 | * @param tolerance If the goal is obstructed, how many meters the planner can 65 | * relax the constraint in x and y before failing 66 | * @param plan The plan... filled by the planner 67 | * @param cost The cost for the the plan 68 | * @param message Optional more detailed outcome as a string 69 | * @return Result code as described on GetPath action result. (see GetPath.action) 70 | */ 71 | virtual uint32_t makePlan(const geometry_msgs::msg::PoseStamped& start, const geometry_msgs::msg::PoseStamped& goal, 72 | double tolerance, std::vector& plan, double& cost, 73 | std::string& message) = 0; 74 | 75 | /** 76 | * @brief Requests the planner to cancel, e.g. if it takes too much time. 77 | * @return True if a cancel has been successfully requested, false if not 78 | * implemented. 79 | */ 80 | virtual bool cancel() = 0; 81 | 82 | /** 83 | * @brief Initializes the planner plugin with a user configured name and a shared pointer to the mesh map 84 | * @param name The user configured name, which is used as namespace for parameters, etc. 85 | * @param mesh_map_ptr A shared pointer to the mesh map instance to access attributes and helper functions, etc. 86 | * @return true if the plugin has been initialized successfully 87 | */ 88 | virtual bool initialize(const std::string& name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) = 0; 89 | 90 | protected: 91 | MeshPlanner() {}; 92 | }; 93 | 94 | } /* namespace mbf_mesh_core */ 95 | 96 | #endif /* MESH_MESH_CORE__MESH_PLANNER_H */ 97 | -------------------------------------------------------------------------------- /mbf_mesh_core/include/mbf_mesh_core/mesh_recovery.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * author: Sebastian Pütz 34 | * 35 | */ 36 | 37 | #ifndef MBF_MESH_CORE__ABSTRACT_RECOVERY_H 38 | #define MBF_MESH_CORE__ABSTRACT_RECOVERY_H 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | namespace mbf_mesh_core 47 | { 48 | /** 49 | * @class MeshRecovery 50 | * @brief Provides an interface for recovery behaviors used in navigation. 51 | * All recovery behaviors written as plugins for the navigation stack must 52 | * adhere to this interface. 53 | */ 54 | class MeshRecovery : public mbf_abstract_core::AbstractRecovery 55 | { 56 | public: 57 | typedef std::shared_ptr<::mbf_mesh_core::MeshRecovery> Ptr; 58 | 59 | /** 60 | * @brief Runs the AbstractRecovery 61 | * @param message The recovery behavior could set, the message should 62 | * correspond to the return value 63 | * @return An outcome which will be hand over to the action result. 64 | */ 65 | virtual uint32_t runBehavior(std::string& message) = 0; 66 | 67 | MeshRecovery() {}; 68 | 69 | /** 70 | * @brief Virtual destructor for the interface 71 | */ 72 | virtual ~MeshRecovery() {}; 73 | 74 | /** 75 | * @brief Requests the recovery behavior to cancel, e.g. if it takes too much 76 | * time. 77 | * @return True if a cancel has been successfully requested, false if not 78 | * implemented. 79 | */ 80 | virtual bool cancel() = 0; 81 | 82 | /** 83 | * @brief Initializes the recovery plugin with a name, a tf pointer and a mesh map pointer 84 | * @param name The recovery behavior plugin name, defined by the user. It defines the plugins namespace 85 | * @param tf_ptr A shared pointer to a transformation buffer 86 | * @param mesh_map_ptr A shared pointer to the mesh map 87 | * @return true if the plugin has been initialized successfully 88 | */ 89 | virtual bool initialize(const std::string& name, const std::shared_ptr& tf_ptr, 90 | const std::shared_ptr& mesh_map_ptr, 91 | const rclcpp::Node::SharedPtr& node) = 0; 92 | 93 | }; 94 | }; /* namespace mbf_mesh_core */ 95 | 96 | #endif /* MBF_MESH_CORE__ABSTRACT_RECOVERY_H */ 97 | -------------------------------------------------------------------------------- /mbf_mesh_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mbf_mesh_core 4 | 2.0.0 5 | The mbf_mesh_core package 6 | 7 | Matthias Holoch 8 | Sebastian Pütz 9 | 10 | BSD-3 11 | Sebastian Pütz 12 | 13 | mbf_abstract_core 14 | mesh_map 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /mbf_mesh_nav/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mbf_mesh_nav 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | 12 | 1.0.0 (2020-12-16) 13 | ------------------ 14 | * Initial release 15 | -------------------------------------------------------------------------------- /mbf_mesh_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mbf_mesh_nav) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | # DEFAULT RELEASE 9 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 10 | if (NOT CMAKE_BUILD_TYPE) 11 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 12 | endif() 13 | endif() 14 | 15 | find_package(ament_cmake_ros REQUIRED) 16 | # ROS deps 17 | set(dependencies 18 | geometry_msgs 19 | mbf_abstract_nav 20 | mbf_simple_core 21 | mbf_mesh_core 22 | mbf_msgs 23 | mesh_map 24 | nav_msgs 25 | pluginlib 26 | rclcpp 27 | std_srvs 28 | ) 29 | foreach(dep IN LISTS dependencies) 30 | find_package(${dep} REQUIRED) 31 | endforeach() 32 | 33 | include_directories( 34 | include 35 | ) 36 | 37 | find_package(LVR2 REQUIRED) 38 | find_package(MPI) 39 | find_package(PkgConfig REQUIRED) 40 | pkg_check_modules(JSONCPP jsoncpp) 41 | 42 | include_directories( 43 | include 44 | ) 45 | 46 | add_library(mbf_mesh_server 47 | src/mesh_controller_execution.cpp 48 | src/mesh_navigation_server.cpp 49 | src/mesh_planner_execution.cpp 50 | src/mesh_recovery_execution.cpp 51 | ) 52 | target_include_directories(mbf_mesh_server PUBLIC 53 | $ 54 | $ 55 | ${LVR2_INCLUDE_DIRS}) 56 | ament_target_dependencies(mbf_mesh_server ${dependencies}) 57 | target_link_libraries(mbf_mesh_server 58 | ${JSONCPP_LIBRARIES} 59 | ${LVR2_LIBRARIES} 60 | ${MPI_CXX_LIBRARIES} 61 | ) 62 | 63 | add_executable(${PROJECT_NAME} src/mbf_mesh_nav.cpp) 64 | ament_target_dependencies(${PROJECT_NAME} ${dependencies} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 65 | target_include_directories(${PROJECT_NAME} PUBLIC 66 | $ 67 | $ 68 | ${LVR2_INCLUDE_DIRS}) 69 | target_link_libraries(${PROJECT_NAME} 70 | ${LVR2_LIBRARIES} 71 | ${MPI_CXX_LIBRARIES} 72 | ${JSONCPP_LIBRARIES} 73 | mbf_mesh_server 74 | ) 75 | 76 | install(DIRECTORY include/ 77 | DESTINATION include 78 | ) 79 | 80 | install(TARGETS mbf_mesh_server 81 | LIBRARY DESTINATION lib 82 | ARCHIVE DESTINATION lib 83 | RUNTIME DESTINATION bin 84 | ) 85 | install(TARGETS ${PROJECT_NAME} 86 | DESTINATION lib/${PROJECT_NAME} 87 | ) 88 | 89 | ament_export_include_directories(include) 90 | ament_export_libraries(${PROJECT_NAME}) 91 | ament_export_dependencies(${dependencies}) 92 | ament_package() -------------------------------------------------------------------------------- /mbf_mesh_nav/cfg/MoveBaseFlex.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'mbf_mesh_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 mesh navigation specific parameters 11 | add_mbf_abstract_nav_params(gen) 12 | 13 | gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False) 14 | 15 | exit(gen.generate(PACKAGE, "mbf_mesh_nav", "MoveBaseFlex")) 16 | -------------------------------------------------------------------------------- /mbf_mesh_nav/include/mbf_mesh_nav/mesh_controller_execution.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MBF_MESH_NAV__MESH_CONTROLLER_EXECUTION_H 39 | #define MBF_MESH_NAV__MESH_CONTROLLER_EXECUTION_H 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | namespace mbf_mesh_nav 48 | { 49 | /** 50 | * @brief The MeshControllerExecution binds a mesh to the 51 | * AbstractControllerExecution and uses the mbf_mesh_core/MeshController class 52 | * as base plugin interface. 53 | * 54 | * @ingroup controller_execution move_base_server 55 | */ 56 | class MeshControllerExecution : public mbf_abstract_nav::AbstractControllerExecution 57 | { 58 | public: 59 | typedef std::shared_ptr MeshPtr; 60 | 61 | /** 62 | * @brief Constructor 63 | * @param name The user defined name of the corresponding 64 | * plugin 65 | * @param controller_ptr The shared pointer to the plugin object 66 | * @param vel_pub The velocity publisher for the controller 67 | * execution 68 | * @param goal_pub The current goal publisher fir the controller 69 | * execution 70 | * @param tf_listener_ptr A shared pointer to the transform listener 71 | * @param mesh_ptr A pointer to the mesh map object 72 | * @param config The current config object 73 | * @param setup_fn A setup function called before execution 74 | * @param cleanup_fn A cleanup function called after execution 75 | */ 76 | MeshControllerExecution(const std::string& name, const mbf_mesh_core::MeshController::Ptr& controller_ptr, 77 | const mbf_utility::RobotInformation::ConstPtr& robot_info, 78 | const rclcpp::Publisher::SharedPtr& vel_pub, const rclcpp::Publisher::SharedPtr& goal_pub, 79 | const MeshPtr& mesh_ptr, const rclcpp::Node::SharedPtr& node); 80 | 81 | /** 82 | * @brief Destructor 83 | */ 84 | virtual ~MeshControllerExecution(); 85 | 86 | protected: 87 | /** 88 | * @brief Request plugin for a new velocity command. We override this method 89 | * so we can lock the local mesh before calling the planner. 90 | * @param robot_pose The current pose of the robot. 91 | * @param robot_velocity The current velocity of the robot. 92 | * @param cmd_vel Will be filled with the velocity command to be 93 | * passed to the robot base. 94 | * @param message Optional more detailed outcome as a string. 95 | * @return Result code as described in the ExePath action 96 | * result and plugin's header. 97 | */ 98 | virtual uint32_t computeVelocityCmd(const geometry_msgs::msg::PoseStamped& robot_pose, 99 | const geometry_msgs::msg::TwistStamped& robot_velocity, 100 | geometry_msgs::msg::TwistStamped& vel_cmd, std::string& message); 101 | 102 | private: 103 | //! mesh for 3d navigation planning 104 | const MeshPtr mesh_ptr_; 105 | 106 | //! Whether to lock mesh before calling the controller 107 | bool lock_mesh_; 108 | 109 | //! name of the controller plugin assigned by the class loader 110 | std::string controller_name_; 111 | }; 112 | 113 | } /* namespace mbf_mesh_nav */ 114 | 115 | #endif /* MBF_MESH_NAV__MESH_CONTROLLER_EXECUTION_H */ 116 | -------------------------------------------------------------------------------- /mbf_mesh_nav/include/mbf_mesh_nav/mesh_navigation_server.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MBF_MESH_NAV__MESH_NAVIGATION_SERVER_H 39 | #define MBF_MESH_NAV__MESH_NAVIGATION_SERVER_H 40 | 41 | #include 42 | 43 | #include 44 | 45 | #include "mesh_controller_execution.h" 46 | #include "mesh_planner_execution.h" 47 | #include "mesh_recovery_execution.h" 48 | 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | 58 | #include 59 | 60 | namespace mbf_mesh_nav 61 | { 62 | /** 63 | * @defgroup move_base_server Move Base Server 64 | * @brief Classes belonging to the Move Base Server level. 65 | */ 66 | 67 | /** 68 | * @brief The MeshNavigationServer makes Move Base Flex backwards compatible to 69 | * the old move_base. It combines the execution classes which use the 70 | * nav_core/BaseLocalPlanner, nav_core/BaseMeshPlanner and the 71 | * nav_core/RecoveryBehavior base classes as plugin interfaces. These 72 | * plugin interface are the same for the old move_base 73 | * 74 | * Supports both mesh_core and simple_core plugins. 75 | * 76 | * @ingroup navigation_server move_base_server 77 | */ 78 | class MeshNavigationServer : public mbf_abstract_nav::AbstractNavigationServer 79 | { 80 | public: 81 | typedef std::shared_ptr MeshPtr; 82 | 83 | typedef std::shared_ptr Ptr; 84 | 85 | /** 86 | * @brief Constructor 87 | * @param tf_listener_ptr Shared pointer to a common TransformListener 88 | */ 89 | MeshNavigationServer(const TFPtr& tf_listener_ptr, const rclcpp::Node::SharedPtr& node); 90 | 91 | /** 92 | * @brief Destructor 93 | */ 94 | virtual ~MeshNavigationServer(); 95 | 96 | virtual void stop(); 97 | 98 | private: 99 | //! shared pointer to a new @ref planner_execution "PlannerExecution" 100 | virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr 101 | newPlannerExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractPlanner::Ptr plugin_ptr); 102 | 103 | //! shared pointer to a new @ref controller_execution "ControllerExecution" 104 | virtual mbf_abstract_nav::AbstractControllerExecution::Ptr 105 | newControllerExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractController::Ptr plugin_ptr); 106 | 107 | //! shared pointer to a new @ref recovery_execution "RecoveryExecution" 108 | virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr 109 | newRecoveryExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractRecovery::Ptr plugin_ptr); 110 | 111 | /** 112 | * @brief Loads the plugin associated with the given planner_type parameter. 113 | * @param planner_type The type of the planner plugin to load. 114 | * @return true, if the local planner plugin was successfully loaded. 115 | */ 116 | virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string& planner_type); 117 | 118 | /** 119 | * @brief Initializes the controller plugin with its name and pointer to the 120 | * mesh 121 | * @param name The name of the planner 122 | * @param planner_ptr pointer to the planner object which corresponds to the 123 | * name param 124 | * @return true if init succeeded, false otherwise 125 | */ 126 | virtual bool initializePlannerPlugin(const std::string& name, 127 | const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr); 128 | 129 | /** 130 | * @brief Loads the plugin associated with the given controller type parameter 131 | * @param controller_type The type of the controller plugin 132 | * @return A shared pointer to a new loaded controller, if the controller 133 | * plugin was loaded successfully, an empty pointer otherwise. 134 | */ 135 | virtual mbf_abstract_core::AbstractController::Ptr loadControllerPlugin(const std::string& controller_type); 136 | 137 | /** 138 | * @brief Initializes the controller plugin with its name, a pointer to the 139 | * TransformListener and pointer to the mesh 140 | * @param name The name of the controller 141 | * @param controller_ptr pointer to the controller object which corresponds to 142 | * the name param 143 | * @return true if init succeeded, false otherwise 144 | */ 145 | virtual bool initializeControllerPlugin(const std::string& name, 146 | const mbf_abstract_core::AbstractController::Ptr& controller_ptr); 147 | 148 | /** 149 | * @brief Loads a Recovery plugin associated with given recovery type 150 | * parameter 151 | * @param recovery_name The name of the Recovery plugin 152 | * @return A shared pointer to a Recovery plugin, if the plugin was loaded 153 | * successfully, an empty pointer otherwise. 154 | */ 155 | virtual mbf_abstract_core::AbstractRecovery::Ptr loadRecoveryPlugin(const std::string& recovery_type); 156 | 157 | /** 158 | * @brief Initializes a recovery behavior plugin with its name and pointers to 159 | * the global and local meshs 160 | * @param name The name of the recovery behavior 161 | * @param behavior_ptr pointer to the recovery behavior object which 162 | * corresponds to the name param 163 | * @return true if init succeeded, false otherwise 164 | */ 165 | virtual bool initializeRecoveryPlugin(const std::string& name, 166 | const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr); 167 | 168 | /** 169 | * @brief Callback method for the check_pose_cost service 170 | * @param request Request object, see the mbf_msgs/srv/CheckPose service 171 | * definition file. 172 | * @param response Response object, see the mbf_msgs/srv/CheckPose service 173 | * definition file. 174 | */ 175 | void callServiceCheckPoseCost(std::shared_ptr request_header, std::shared_ptr request, std::shared_ptr response); 176 | 177 | /** 178 | * @brief Callback method for the check_path_cost service 179 | * @param request Request object, see the mbf_msgs/srv/CheckPath service 180 | * definition file. 181 | * @param response Response object, see the mbf_msgs/srv/CheckPath service 182 | * definition file. 183 | */ 184 | void callServiceCheckPathCost(std::shared_ptr request_header, std::shared_ptr request, std::shared_ptr response); 185 | 186 | /** 187 | * @brief Callback method for the make_plan service 188 | * @param request Empty request object. 189 | * @param response Empty response object. 190 | */ 191 | void callServiceClearMesh(std::shared_ptr request_header, std::shared_ptr request, std::shared_ptr response); 192 | 193 | //! plugin class loader for recovery behaviors plugins 194 | pluginlib::ClassLoader recovery_plugin_loader_; 195 | pluginlib::ClassLoader simple_recovery_plugin_loader_; 196 | 197 | //! plugin class loader for controller plugins 198 | pluginlib::ClassLoader controller_plugin_loader_; 199 | pluginlib::ClassLoader simple_controller_plugin_loader_; 200 | 201 | //! plugin class loader for planner plugins 202 | pluginlib::ClassLoader planner_plugin_loader_; 203 | pluginlib::ClassLoader simple_planner_plugin_loader_; 204 | 205 | //! Shared pointer to the common global mesh 206 | MeshPtr mesh_ptr_; 207 | 208 | //! Service Server to clear the mesh 209 | rclcpp::Service::SharedPtr clear_mesh_srv_; 210 | 211 | //! Service Server for the check_pose_cost service 212 | rclcpp::Service::SharedPtr check_pose_cost_srv_; 213 | 214 | //! Service Server for the check_path_cost service 215 | rclcpp::Service::SharedPtr check_path_cost_srv_; 216 | 217 | //! Start/stop meshs mutex; concurrent calls to start can lead to segfault 218 | std::mutex check_meshs_mutex_; 219 | }; 220 | 221 | } /* namespace mbf_mesh_nav */ 222 | 223 | #endif /* MBF_MESH_NAV__MESH_NAVIGATION_SERVER_H */ 224 | -------------------------------------------------------------------------------- /mbf_mesh_nav/include/mbf_mesh_nav/mesh_planner_execution.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MBF_MESH_NAV__MESH_PLANNER_EXECUTION_H 39 | #define MBF_MESH_NAV__MESH_PLANNER_EXECUTION_H 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | namespace mbf_mesh_nav 46 | { 47 | /** 48 | * @brief The MeshPlannerExecution binds a global mesh to the 49 | * AbstractPlannerExecution and uses the nav_core/BaseMeshPlanner class as base 50 | * plugin interface. This class makes move_base_flex compatible to the old 51 | * move_base. 52 | * 53 | * @ingroup planner_execution move_base_server 54 | */ 55 | class MeshPlannerExecution : public mbf_abstract_nav::AbstractPlannerExecution 56 | { 57 | public: 58 | typedef std::shared_ptr MeshPtr; 59 | 60 | /** 61 | * @brief Constructor 62 | * @param condition Thread sleep condition variable, to wake up connected 63 | * threads 64 | * @param mesh Shared pointer to the mesh. 65 | */ 66 | MeshPlannerExecution(const std::string name, const mbf_mesh_core::MeshPlanner::Ptr& planner_ptr, const mbf_utility::RobotInformation::ConstPtr& robot_info, const MeshPtr& mesh, const rclcpp::Node::SharedPtr& node); 67 | 68 | /** 69 | * @brief Destructor 70 | */ 71 | virtual ~MeshPlannerExecution(); 72 | 73 | private: 74 | /** 75 | * @brief Calls the planner plugin to make a plan from the start pose to the 76 | * goal pose with the given tolerance, if a goal tolerance is enabled in the 77 | * planner plugin. 78 | * @param start The start pose for planning 79 | * @param goal The goal pose for planning 80 | * @param tolerance The goal tolerance 81 | * @param plan The computed plan by the plugin 82 | * @param cost The computed costs for the corresponding plan 83 | * @param message An optional message which should correspond with the 84 | * returned outcome 85 | * @return An outcome number, see also the action definition in the 86 | * GetPath.action file 87 | */ 88 | virtual uint32_t makePlan( 89 | const geometry_msgs::msg::PoseStamped &start, 90 | const geometry_msgs::msg::PoseStamped &goal, 91 | double tolerance, 92 | std::vector &plan, 93 | double &cost, 94 | std::string &message); 95 | 96 | //! Shared pointer to the mesh for 3d navigation planning 97 | const MeshPtr mesh_ptr_; 98 | 99 | //! Whether to lock mesh before calling the planner (see issue #4 for details) 100 | bool lock_mesh_; 101 | 102 | //! Name of the planner assigned by the class loader 103 | std::string planner_name_; 104 | }; 105 | 106 | } /* namespace mbf_mesh_nav */ 107 | 108 | #endif /* MBF_MESH_NAV__MESH_PLANNER_EXECUTION_H */ 109 | -------------------------------------------------------------------------------- /mbf_mesh_nav/include/mbf_mesh_nav/mesh_recovery_execution.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MBF_MESH_NAV__MESH_RECOVERY_EXECUTION_H 39 | #define MBF_MESH_NAV__MESH_RECOVERY_EXECUTION_H 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | namespace mbf_mesh_nav 46 | { 47 | /** 48 | * @brief The MeshRecoveryExecution binds a local and a global mesh to the 49 | * AbstractRecoveryExecution and uses the nav_core/MeshRecovery class as base 50 | * plugin interface. This class makes move_base_flex compatible to the old 51 | * move_base. 52 | * 53 | * @ingroup recovery_execution move_base_server 54 | */ 55 | class MeshRecoveryExecution : public mbf_abstract_nav::AbstractRecoveryExecution 56 | { 57 | public: 58 | typedef std::shared_ptr MeshPtr; 59 | typedef std::shared_ptr Ptr; 60 | 61 | /** 62 | * @brief Constructor 63 | * @param tf_listener_ptr Shared pointer to a common tf listener 64 | * @param global_mesh Shared pointer to the global mesh. 65 | * @param local_mesh Shared pointer to the local mesh. 66 | */ 67 | MeshRecoveryExecution(const std::string name, 68 | const mbf_mesh_core::MeshRecovery::Ptr& recovery_ptr, 69 | const mbf_utility::RobotInformation::ConstPtr& robot_info, 70 | const MeshPtr& mesh_ptr, 71 | const rclcpp::Node::SharedPtr& node); 72 | /** 73 | * Destructor 74 | */ 75 | virtual ~MeshRecoveryExecution(); 76 | 77 | protected: 78 | //! Shared pointer to the mesh for 3D navigation planning 79 | const MeshPtr mesh_ptr_; 80 | }; 81 | 82 | } /* namespace mbf_mesh_nav */ 83 | 84 | #endif /* MBF_MESH_NAV__MESH_RECOVERY_EXECUTION_H */ 85 | -------------------------------------------------------------------------------- /mbf_mesh_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mbf_mesh_nav 4 | 2.0.0 5 | The mbf_mesh_nav package 6 | 7 | Matthias Holoch 8 | Sebastian Pütz 9 | 10 | BSD-3 11 | Sebastian Pütz 12 | 13 | geometry_msgs 14 | mbf_abstract_nav 15 | mbf_simple_core 16 | mbf_mesh_core 17 | mbf_msgs 18 | mesh_map 19 | nav_msgs 20 | pluginlib 21 | rclcpp 22 | std_srvs 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /mbf_mesh_nav/src/mbf_mesh_nav.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | */ 36 | 37 | #include "mbf_mesh_nav/mesh_navigation_server.h" 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | mbf_mesh_nav::MeshNavigationServer::Ptr mesh_nav_srv_ptr; 44 | rclcpp::Node::SharedPtr node; 45 | 46 | void sigintHandler(int sig) 47 | { 48 | RCLCPP_INFO_STREAM(node->get_logger(), "Shutdown mesh navigation server."); 49 | if (mesh_nav_srv_ptr) 50 | { 51 | mesh_nav_srv_ptr->stop(); 52 | } 53 | rclcpp::shutdown(); 54 | } 55 | 56 | int main(int argc, char** argv) 57 | { 58 | rclcpp::init(argc, argv, rclcpp::InitOptions(), rclcpp::SignalHandlerOptions::None); 59 | 60 | node = std::make_shared("mbf_mesh_nav"); 61 | const double cache_time = node->declare_parameter("tf_cache_time", 10.0); 62 | 63 | TFPtr tf_buffer_ptr = std::make_shared(node->get_clock(), tf2::durationFromSec(cache_time)); 64 | tf2_ros::TransformListener tf_listener(*tf_buffer_ptr); 65 | mesh_nav_srv_ptr = std::make_shared(tf_buffer_ptr, node); 66 | 67 | signal(SIGINT, sigintHandler); 68 | 69 | rclcpp::spin(node); 70 | 71 | return EXIT_SUCCESS; 72 | } 73 | -------------------------------------------------------------------------------- /mbf_mesh_nav/src/mesh_controller_execution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | #include "mbf_mesh_nav/mesh_controller_execution.h" 38 | 39 | namespace mbf_mesh_nav 40 | { 41 | MeshControllerExecution::MeshControllerExecution(const std::string& name, 42 | const mbf_mesh_core::MeshController::Ptr& controller_ptr, 43 | const mbf_utility::RobotInformation::ConstPtr& robot_info, 44 | const rclcpp::Publisher::SharedPtr& vel_pub, 45 | const rclcpp::Publisher::SharedPtr& goal_pub, 46 | const MeshPtr& mesh_ptr, 47 | const rclcpp::Node::SharedPtr& node) 48 | : AbstractControllerExecution(name, controller_ptr, robot_info, vel_pub, goal_pub, node) 49 | , mesh_ptr_(mesh_ptr) 50 | { 51 | lock_mesh_ = node_handle_->declare_parameter("controller_lock_mesh", true); 52 | } 53 | 54 | MeshControllerExecution::~MeshControllerExecution() 55 | { 56 | } 57 | 58 | uint32_t MeshControllerExecution::computeVelocityCmd(const geometry_msgs::msg::PoseStamped& robot_pose, 59 | const geometry_msgs::msg::TwistStamped& robot_velocity, 60 | geometry_msgs::msg::TwistStamped& vel_cmd, std::string& message) 61 | { 62 | // Lock the mesh while planning, but following issue #4, we allow to move the 63 | // responsibility to the planner itself 64 | if (lock_mesh_) 65 | { 66 | // TODO 67 | // boost::lock_guard lock(*(mesh_ptr_->getMutex())); 68 | // return controller_->computeVelocityCommands(robot_pose, robot_velocity, 69 | // vel_cmd, message); 70 | } 71 | return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message); 72 | } 73 | 74 | } /* namespace mbf_mesh_nav */ 75 | -------------------------------------------------------------------------------- /mbf_mesh_nav/src/mesh_planner_execution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | #include "mbf_mesh_nav/mesh_planner_execution.h" 38 | 39 | namespace mbf_mesh_nav 40 | { 41 | MeshPlannerExecution::MeshPlannerExecution(const std::string name, 42 | const mbf_mesh_core::MeshPlanner::Ptr& planner_ptr, 43 | const mbf_utility::RobotInformation::ConstPtr& robot_info, 44 | const MeshPtr& mesh_ptr, 45 | const rclcpp::Node::SharedPtr& node) 46 | : AbstractPlannerExecution(name, planner_ptr, robot_info, node), mesh_ptr_(mesh_ptr) 47 | { 48 | lock_mesh_ = node_->declare_parameter("planner_lock_mesh", true); 49 | } 50 | 51 | MeshPlannerExecution::~MeshPlannerExecution() 52 | { 53 | } 54 | 55 | uint32_t MeshPlannerExecution::makePlan(const geometry_msgs::msg::PoseStamped &start, 56 | const geometry_msgs::msg::PoseStamped &goal, 57 | double tolerance, 58 | std::vector &plan, 59 | double &cost, 60 | std::string &message) 61 | { 62 | rclcpp::Time start_time = node_->now(); 63 | uint32_t outcome = planner_->makePlan(start, goal, tolerance, plan, cost, message); 64 | RCLCPP_INFO_STREAM(node_->get_logger(), "Runtime of " << plugin_name_ << ":" << (node_->now() - start_time).nanoseconds() * 1e-6 << "ms"); 65 | return outcome; 66 | } 67 | 68 | } /* namespace mbf_mesh_nav */ 69 | -------------------------------------------------------------------------------- /mbf_mesh_nav/src/mesh_recovery_execution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | #include "mbf_mesh_nav/mesh_recovery_execution.h" 38 | 39 | namespace mbf_mesh_nav 40 | { 41 | MeshRecoveryExecution::MeshRecoveryExecution(const std::string name, 42 | const mbf_mesh_core::MeshRecovery::Ptr& recovery_ptr, 43 | const mbf_utility::RobotInformation::ConstPtr& robot_info, 44 | const MeshPtr& mesh_ptr, 45 | const rclcpp::Node::SharedPtr& node) 46 | : AbstractRecoveryExecution(name, recovery_ptr, robot_info, node), mesh_ptr_(mesh_ptr) 47 | { 48 | } 49 | 50 | MeshRecoveryExecution::~MeshRecoveryExecution() 51 | { 52 | } 53 | 54 | } /* namespace mbf_mesh_nav */ 55 | -------------------------------------------------------------------------------- /mesh_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | 12 | 1.0.0 (2020-12-16) 13 | ------------------ 14 | * Initial release 15 | -------------------------------------------------------------------------------- /mesh_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mesh_controller) 3 | 4 | # DEFAULT RELEASE 5 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 8 | endif() 9 | endif() 10 | 11 | find_package(ament_cmake_ros REQUIRED) 12 | find_package(example_interfaces REQUIRED) 13 | find_package(mbf_mesh_core REQUIRED) 14 | find_package(mbf_msgs REQUIRED) 15 | find_package(mbf_utility REQUIRED) 16 | find_package(mesh_map REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(tf2_geometry_msgs REQUIRED) 19 | find_package(LVR2 REQUIRED) 20 | find_package(MPI) 21 | 22 | pluginlib_export_plugin_description_file(mbf_mesh_core mesh_controller.xml) 23 | 24 | add_library(${PROJECT_NAME} src/mesh_controller.cpp) 25 | target_include_directories(${PROJECT_NAME} PUBLIC 26 | $ 27 | $ 28 | ${LVR2_INCLUDE_DIRS}) 29 | target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_CONTROLLER_BUILDING_LIBRARY") 30 | ament_target_dependencies(${PROJECT_NAME} example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs) 31 | target_link_libraries(${PROJECT_NAME} 32 | ${LVR2_LIBRARIES} 33 | ${MPI_CXX_LIBRARIES} 34 | ) 35 | 36 | install(DIRECTORY include DESTINATION include) 37 | install(TARGETS ${PROJECT_NAME} 38 | EXPORT export_${PROJECT_NAME} 39 | ARCHIVE DESTINATION lib 40 | LIBRARY DESTINATION lib 41 | RUNTIME DESTINATION bin 42 | ) 43 | 44 | ament_export_include_directories(include) 45 | ament_export_libraries(${PROJECT_NAME}) 46 | ament_export_targets(export_${PROJECT_NAME}) 47 | ament_export_dependencies(example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs) 48 | ament_package() -------------------------------------------------------------------------------- /mesh_controller/cfg/MeshController.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import * 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("max_lin_velocity", double_t, 0, "Defines the maximum linear velocity", 1.0, 0.0, 5.0) 8 | gen.add("max_ang_velocity", double_t, 0, "Defines the maximum angular velocity", 0.5, 0.0, 2.0) 9 | gen.add("arrival_fading", double_t, 0, "Distance to goal position where the robot starts to fade down the linear velocity", 0.5, 0.0, 5.0) 10 | gen.add("ang_vel_factor", double_t, 0, "Factor for angular velocity", 1.0, 0.1, 10.0) 11 | gen.add("lin_vel_factor", double_t, 0, "Factor for linear velocity", 1.0, 0.1, 10.0) 12 | gen.add("max_angle", double_t, 0, "The maximum angle for the linear velocity function", 20.0, 1.0, 180.0) 13 | gen.add("max_search_radius", double_t, 0, "The maximum radius in which to search for a consecutive neighbour face", 0.4, 0.01, 2.0) 14 | gen.add("max_search_distance", double_t, 0, "The maximum distance from the surface which is accepted for projection", 0.4, 0.01, 2.0) 15 | 16 | exit(gen.generate("mesh_controller", "mesh_controller", "MeshController")) 17 | -------------------------------------------------------------------------------- /mesh_controller/include/mesh_controller/mesh_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | */ 34 | 35 | #ifndef MESH_NAVIGATION__MESH_CONTROLLER_H 36 | #define MESH_NAVIGATION__MESH_CONTROLLER_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace mesh_controller 45 | { 46 | class MeshController : public mbf_mesh_core::MeshController 47 | { 48 | public: 49 | 50 | //! shared pointer typedef to simplify pointer access of the mesh controller 51 | typedef std::shared_ptr Ptr; 52 | 53 | /** 54 | * @brief Constructor 55 | */ 56 | MeshController(); 57 | 58 | /** 59 | * @brief Destructor 60 | */ 61 | virtual ~MeshController(); 62 | 63 | /** 64 | * @brief Given the current position, orientation, and velocity of the robot, 65 | * compute the next velocity commands to move the robot towards the goal. 66 | * @param pose The current pose of the robot 67 | * @param velocity The current velocity of the robot 68 | * @param cmd_vel Computed velocity command 69 | * @param message Detailed outcome as string message 70 | * @return An mbf_msgs/ExePathResult outcome code 71 | */ 72 | virtual uint32_t computeVelocityCommands(const geometry_msgs::msg::PoseStamped& pose, 73 | const geometry_msgs::msg::TwistStamped& velocity, 74 | geometry_msgs::msg::TwistStamped& cmd_vel, std::string& message) override; 75 | 76 | /** 77 | * @brief Checks if the robot reached to goal pose 78 | * @param pose The current pose of the robot 79 | * @param angle_tolerance The angle tolerance in which the current pose will 80 | * be partly accepted as reached goal 81 | * @param dist_tolerance The distance tolerance in which the current pose will 82 | * be partly accepted as reached goal 83 | * @return true if the goal is reached 84 | */ 85 | virtual bool isGoalReached(double dist_tolerance, double angle_tolerance) override; 86 | 87 | /** 88 | * @brief Sets the current plan to follow, it also sets the vector field 89 | * @param plan The plan to follow 90 | * @return true if the plan was set successfully, false otherwise 91 | */ 92 | virtual bool setPlan(const std::vector& plan) override; 93 | 94 | /** 95 | * @brief Requests the planner to cancel, e.g. if it takes too much time 96 | * @return True if cancel has been successfully requested, false otherwise 97 | */ 98 | virtual bool cancel() override; 99 | 100 | /** 101 | * @brief Initializes the controller plugin with a name, a tf pointer and a mesh map pointer 102 | * @param plugin_name The controller plugin name, defined by the user. It defines the controller namespace 103 | * @param tf_ptr A shared pointer to a transformation buffer 104 | * @param mesh_map_ptr A shared pointer to the mesh map 105 | * @return true if the plugin has been initialized successfully 106 | */ 107 | virtual bool initialize(const std::string& plugin_name, 108 | const std::shared_ptr& tf_ptr, 109 | const std::shared_ptr& mesh_map_ptr, 110 | const rclcpp::Node::SharedPtr& node) override; 111 | 112 | /** 113 | * Converts the orientation of a geometry_msgs/PoseStamped message to a direction vector 114 | * @param pose the pose to convert 115 | * @return direction normal vector 116 | */ 117 | mesh_map::Normal poseToDirectionVector( 118 | const geometry_msgs::msg::PoseStamped& pose, 119 | const tf2::Vector3& axis=tf2::Vector3(1,0,0)); 120 | 121 | 122 | /** 123 | * Converts the position of a geometry_msgs/PoseStamped message to a position vector 124 | * @param pose the pose to convert 125 | * @return position vector 126 | */ 127 | mesh_map::Vector poseToPositionVector( 128 | const geometry_msgs::msg::PoseStamped& pose); 129 | 130 | /** 131 | * A normal distribution / gaussian function 132 | * @param sigma_squared The squared sigma / variance 133 | * @param value The value to be evaluated 134 | * @return the gauss faction value 135 | */ 136 | float gaussValue(const float& sigma_squared, const float& value); 137 | 138 | /** 139 | * Computes the angular and linear velocity 140 | * @param robot_pos robot position 141 | * @param robot_dir robot orientation 142 | * @param mesh_dir supposed orientation 143 | * @param mesh_cost cost value at robot position 144 | * @return angular and linear velocity 145 | */ 146 | std::array naiveControl( 147 | const mesh_map::Vector& robot_pos, 148 | const mesh_map::Normal& robot_dir, 149 | const mesh_map::Vector& mesh_dir, 150 | const mesh_map::Normal& mesh_normal, 151 | const float& mesh_cost); 152 | 153 | /** 154 | * @brief reconfigure callback function which is called if a dynamic reconfiguration were triggered. 155 | */ 156 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 157 | 158 | private: 159 | //! shared pointer to node in which this plugin runs 160 | rclcpp::Node::SharedPtr node_; 161 | 162 | //! the user defined plugin name 163 | std::string name_; 164 | 165 | //! shared pointer to the used mesh map 166 | std::shared_ptr map_ptr_; 167 | 168 | //! the current set plan 169 | vector current_plan_; 170 | 171 | //! the goal and robot pose 172 | mesh_map::Vector goal_pos_, robot_pos_; 173 | 174 | //! the goal's and robot's orientation 175 | mesh_map::Normal goal_dir_, robot_dir_; 176 | 177 | //! The triangle on which the robot is located 178 | lvr2::OptionalFaceHandle current_face_; 179 | 180 | //! The vector field to the goal. 181 | lvr2::DenseVertexMap vector_map_; 182 | 183 | //! publishes the angle between the robots orientation and the goal vector field for debug purposes 184 | rclcpp::Publisher::SharedPtr angle_pub_; 185 | 186 | //! flag to handle cancel requests 187 | std::atomic_bool cancel_requested_; 188 | 189 | // handle of callback for changing parameters dynamically 190 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_; 191 | 192 | struct { 193 | double max_lin_velocity = 1.0; 194 | double max_ang_velocity = 0.5; 195 | double arrival_fading = 0.5; 196 | double ang_vel_factor = 1.0; 197 | double lin_vel_factor = 1.0; 198 | double max_angle = 20.0; 199 | double max_search_radius = 0.4; 200 | double max_search_distance = 0.4; 201 | } config_; 202 | }; 203 | 204 | } /* namespace mesh_controller */ 205 | #endif /* MESH_NAVIGATION__MESH_CONTROLLER_H */ 206 | -------------------------------------------------------------------------------- /mesh_controller/mesh_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | A mesh controller for mbf_mesh_nav 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /mesh_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_controller 4 | 2.0.0 5 | The mesh_controller package 6 | 7 | Matthias Holoch 8 | Sebastian Pütz 9 | BSD-3 10 | Sebastian Pütz 11 | Sabrina Frohn 12 | 13 | mbf_mesh_core 14 | mbf_msgs 15 | mbf_utility 16 | mesh_map 17 | rclcpp 18 | tf2_geometry_msgs 19 | example_interfaces 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /mesh_layers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_layers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | 12 | 1.0.0 (2020-12-16) 13 | ------------------ 14 | * Initial release 15 | -------------------------------------------------------------------------------- /mesh_layers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mesh_layers) 3 | 4 | # DEFAULT RELEASE 5 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 8 | endif() 9 | endif() 10 | 11 | find_package(ament_cmake_ros REQUIRED) 12 | find_package(mesh_map REQUIRED) 13 | find_package(LVR2 REQUIRED) 14 | add_definitions(${LVR2_DEFINITIONS}) 15 | 16 | pluginlib_export_plugin_description_file(mesh_map mesh_layers.xml) 17 | 18 | add_library(${PROJECT_NAME} 19 | src/border_layer.cpp 20 | src/roughness_layer.cpp 21 | src/height_diff_layer.cpp 22 | src/inflation_layer.cpp 23 | src/steepness_layer.cpp 24 | src/ridge_layer.cpp 25 | src/clearance_layer.cpp 26 | ) 27 | include_directories( 28 | include 29 | ${LVR2_INCLUDE_DIRS} 30 | ) 31 | 32 | target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 33 | target_include_directories(${PROJECT_NAME} PUBLIC 34 | $ 35 | $) 36 | 37 | ament_target_dependencies(${PROJECT_NAME} mesh_map LVR2) 38 | target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_LAYERS_BUILDING_LIBRARY") 39 | target_link_libraries(${PROJECT_NAME} 40 | ${LVR2_LIBRARIES} 41 | ) 42 | 43 | install(DIRECTORY include/ 44 | DESTINATION include 45 | ) 46 | 47 | install(TARGETS 48 | ${PROJECT_NAME} 49 | ARCHIVE DESTINATION lib 50 | LIBRARY DESTINATION lib 51 | RUNTIME DESTINATION bin 52 | ) 53 | 54 | ament_export_include_directories(include) 55 | ament_export_libraries(${PROJECT_NAME}) 56 | ament_export_dependencies(mesh_map) 57 | ament_package() 58 | -------------------------------------------------------------------------------- /mesh_layers/include/mesh_layers/border_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024, Alexander Mock 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 | * authors: 34 | * Alexander Mock 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__BORDER_LAYER_H 39 | #define MESH_MAP__BORDER_LAYER_H 40 | 41 | #include 42 | #include 43 | 44 | namespace mesh_layers 45 | { 46 | /** 47 | * @brief Costmap layer which calculates the border costs to avoid mesh borders 48 | */ 49 | class BorderLayer : public mesh_map::AbstractLayer 50 | { 51 | /** 52 | * @brief try read layer from map file 53 | * 54 | * @return true if successul; else false 55 | */ 56 | virtual bool readLayer() override; 57 | 58 | /** 59 | * @brief try to write layer to map file 60 | * 61 | * @return true if successfull; else false 62 | */ 63 | virtual bool writeLayer() override; 64 | 65 | /** 66 | * @brief delivers the default layer value 67 | * 68 | * @return default value used for this layer 69 | */ 70 | virtual float defaultValue() override 71 | { 72 | return std::numeric_limits::infinity(); 73 | } 74 | 75 | /** 76 | * @brief delivers the threshold above which vertices are marked lethal 77 | * 78 | * @return lethal threshold 79 | */ 80 | virtual float threshold() override; 81 | 82 | /** 83 | * @brief calculate the values of this layer 84 | * 85 | * @return true if successfull; else false 86 | */ 87 | virtual bool computeLayer() override; 88 | 89 | /** 90 | * @brief mark vertices with values above the threshold as lethal 91 | * 92 | * @return true if successfull; else false 93 | */ 94 | bool computeLethals(); 95 | 96 | /** 97 | * @brief deliver the current costmap 98 | * 99 | * @return calculated costmap 100 | */ 101 | virtual lvr2::VertexMap& costs() override; 102 | 103 | /** 104 | * @brief deliver set containing all vertices marked as lethal 105 | * 106 | * @return lethal vertices 107 | */ 108 | virtual std::set& lethals() override 109 | { 110 | return lethal_vertices_; 111 | } 112 | 113 | /** 114 | * @brief update set of lethal vertices by adding and removing vertices 115 | * 116 | * @param added_lethal vertices to be marked as lethal 117 | * @param removed_lethal vertices to be removed from the set of lethal vertices 118 | */ 119 | virtual void updateLethal(std::set& added_lethal, std::set& removed_lethal) override {}; 120 | 121 | /** 122 | * @brief initializes this layer plugin 123 | * 124 | * @return true if initialization was successfull; else false 125 | */ 126 | virtual bool initialize() override; 127 | 128 | /** 129 | * @brief callback for incoming param changes 130 | */ 131 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 132 | 133 | // latest costmap 134 | lvr2::DenseVertexMap border_costs_; 135 | // set of all current lethal vertices 136 | std::set lethal_vertices_; 137 | 138 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 139 | struct { 140 | double threshold = 0.5; 141 | double border_cost = 1.0; 142 | double factor = 1.0; 143 | } config_; 144 | }; 145 | 146 | } /* namespace mesh_layers */ 147 | 148 | #endif // MESH_MAP__BORDER_LAYER_H 149 | -------------------------------------------------------------------------------- /mesh_layers/include/mesh_layers/clearance_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024, Justus Braun 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 | * authors: 34 | * Justus Braun 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__FREESPACE_LAYER_H 39 | #define MESH_MAP__FREESPACE_LAYER_H 40 | 41 | #include 42 | #include 43 | 44 | namespace mesh_layers 45 | { 46 | /** 47 | * @brief Costmap layer which calculates the free space in the direction of the vertex normals 48 | */ 49 | class ClearanceLayer : public mesh_map::AbstractLayer 50 | { 51 | /** 52 | * @brief try read layer from map file 53 | * 54 | * @return true if successul; else false 55 | */ 56 | virtual bool readLayer() override; 57 | 58 | /** 59 | * @brief try to write layer to map file 60 | * 61 | * @return true if successfull; else false 62 | */ 63 | virtual bool writeLayer() override; 64 | 65 | /** 66 | * @brief delivers the default layer value 67 | * 68 | * @return default value used for this layer 69 | */ 70 | virtual float defaultValue() override 71 | { 72 | return std::numeric_limits::infinity(); 73 | } 74 | 75 | /** 76 | * @brief delivers the threshold above which vertices are marked lethal 77 | * 78 | * @return lethal threshold 79 | */ 80 | virtual float threshold() override; 81 | 82 | /** 83 | * @brief calculate the values of this layer 84 | * 85 | * @return true if successfull; else false 86 | */ 87 | virtual bool computeLayer() override; 88 | 89 | /** 90 | * @brief deliver the current costmap 91 | * 92 | * @return calculated costmap 93 | */ 94 | virtual lvr2::VertexMap& costs() override; 95 | 96 | /** 97 | * @brief deliver set containing all vertices marked as lethal 98 | * 99 | * @return lethal vertices 100 | */ 101 | virtual std::set& lethals() override 102 | { 103 | return lethal_vertices_; 104 | } 105 | 106 | /** 107 | * @brief update set of lethal vertices by adding and removing vertices 108 | * 109 | * @param added_lethal vertices to be marked as lethal 110 | * @param removed_lethal vertices to be removed from the set of lethal vertices 111 | */ 112 | virtual void updateLethal(std::set& added_lethal, std::set& removed_lethal) override {}; 113 | 114 | /** 115 | * @brief initializes this layer plugin 116 | * 117 | * @return true if initialization was successfull; else false 118 | */ 119 | virtual bool initialize() override; 120 | 121 | /** 122 | * @brief callback for incoming param changes 123 | */ 124 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 125 | 126 | private: 127 | /** 128 | * @brief mark vertices without enough clearance as lethal and compute the costs 129 | * 130 | * @return true if successfull; else false 131 | */ 132 | bool computeLethalsAndCosts(); 133 | 134 | // distance along vertex normal until the next face 135 | lvr2::DenseVertexMap clearance_; 136 | // Actual costs values 137 | lvr2::DenseVertexMap costs_; 138 | // set of all current lethal vertices 139 | std::set lethal_vertices_; 140 | 141 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 142 | struct { 143 | double robot_height = 0.5; 144 | double height_inflation = 0.3; 145 | double factor = 1.0; 146 | } config_; 147 | }; 148 | 149 | } /* namespace mesh_layers */ 150 | 151 | #endif // MESH_MAP__FREESPACE_LAYER_H 152 | -------------------------------------------------------------------------------- /mesh_layers/include/mesh_layers/height_diff_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__HEIGHTDIFF_LAYER_H 39 | #define MESH_MAP__HEIGHTDIFF_LAYER_H 40 | 41 | #include 42 | #include 43 | 44 | namespace mesh_layers 45 | { 46 | /** 47 | * @brief Costmap layer which calculates the height difference to avoid steep regions 48 | */ 49 | class HeightDiffLayer : public mesh_map::AbstractLayer 50 | { 51 | /** 52 | * @brief try read layer from map file 53 | * 54 | * @return true if successul; else false 55 | */ 56 | virtual bool readLayer() override; 57 | 58 | /** 59 | * @brief try to write layer to map file 60 | * 61 | * @return true if successfull; else false 62 | */ 63 | virtual bool writeLayer() override; 64 | 65 | /** 66 | * @brief delivers the default layer value 67 | * 68 | * @return default value used for this layer 69 | */ 70 | virtual float defaultValue() override 71 | { 72 | return std::numeric_limits::infinity(); 73 | } 74 | 75 | /** 76 | * @brief delivers the threshold above which vertices are marked lethal 77 | * 78 | * @return lethal threshold 79 | */ 80 | virtual float threshold() override; 81 | 82 | /** 83 | * @brief calculate the values of this layer 84 | * 85 | * @return true if successfull; else false 86 | */ 87 | virtual bool computeLayer() override; 88 | 89 | /** 90 | * @brief mark vertices with values above the threshold as lethal 91 | * 92 | * @return true if successfull; else false 93 | */ 94 | bool computeLethals(); 95 | 96 | /** 97 | * @brief deliver the current costmap 98 | * 99 | * @return calculated costmap 100 | */ 101 | virtual lvr2::VertexMap& costs() override; 102 | 103 | /** 104 | * @brief deliver set containing all vertices marked as lethal 105 | * 106 | * @return lethal vertices 107 | */ 108 | virtual std::set& lethals() override 109 | { 110 | return lethal_vertices_; 111 | } 112 | 113 | /** 114 | * @brief update set of lethal vertices by adding and removing vertices 115 | * 116 | * @param added_lethal vertices to be marked as lethal 117 | * @param removed_lethal vertices to be removed from the set of lethal vertices 118 | */ 119 | virtual void updateLethal(std::set& added_lethal, std::set& removed_lethal) override {}; 120 | 121 | /** 122 | * @brief initializes this layer plugin 123 | * 124 | * @return true if initialization was successfull; else false 125 | */ 126 | virtual bool initialize() override; 127 | 128 | /** 129 | * @brief callback for incoming param changes 130 | */ 131 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 132 | 133 | // latest costmap 134 | lvr2::DenseVertexMap height_diff_; 135 | // set of all current lethal vertices 136 | std::set lethal_vertices_; 137 | 138 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 139 | struct { 140 | double threshold = 0.185; 141 | double radius = 0.3; 142 | double factor = 1.0; 143 | } config_; 144 | }; 145 | 146 | } /* namespace mesh_layers */ 147 | 148 | #endif // MESH_MAP__HEIGHTDIFF_LAYER_H 149 | -------------------------------------------------------------------------------- /mesh_layers/include/mesh_layers/inflation_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__INFLATION_LAYER_H 39 | #define MESH_MAP__INFLATION_LAYER_H 40 | 41 | #include 42 | #include 43 | 44 | namespace mesh_layers 45 | { 46 | const float EPSILON = 1e-9; 47 | 48 | /** 49 | * @brief Costmap layer which inflates around existing lethal vertices 50 | */ 51 | class InflationLayer : public mesh_map::AbstractLayer 52 | { 53 | /** 54 | * @brief try read layer from map file 55 | * 56 | * @return true if successul; else false 57 | */ 58 | virtual bool readLayer() override; 59 | 60 | /** 61 | * @brief try to write layer to map file 62 | * 63 | * @return true if successfull; else false 64 | */ 65 | virtual bool writeLayer() override; 66 | 67 | /** 68 | * @brief delivers the default layer value 69 | * 70 | * @return default value used for this layer 71 | */ 72 | virtual float defaultValue() override 73 | { 74 | return 0; 75 | } 76 | 77 | /** 78 | * @brief delivers the threshold above which vertices are marked lethal 79 | * 80 | * @return lethal threshold 81 | */ 82 | virtual float threshold() override; 83 | 84 | /** 85 | * @brief inflate around lethal vertices by inflating to neighbours on mesh and using squared distances and assign 86 | * riskiness values to vertices 87 | * 88 | * @param lethals set of current lethal vertices 89 | * @param inflation_radius radius of the inflation 90 | * @param inscribed_radius radius of the inscribed area 91 | * @param inscribed_value value assigned to vertices in inscribed area 92 | * @param lethal_value value assigned to lethal vertices 93 | */ 94 | void lethalCostInflation(const std::set& lethals, const float inflation_radius, 95 | const float inscribed_radius, const float inscribed_value, const float lethal_value); 96 | 97 | inline float computeUpdateSethianMethod(const float& d1, const float& d2, const float& a, const float& b, 98 | const float& dot, const float& F); 99 | 100 | /** 101 | * @brief updates the wavefront 102 | * 103 | * @param distances current distances from the start vertices 104 | * @param predecessors current predecessors of vertices visited during the wave front propagation 105 | * @param max_distance max distance of propagation 106 | * @param edge_weights weights of the edges 107 | * @param fh current face 108 | * @param normal normal of the current face 109 | * @param v1 first vertex of the current face 110 | * @param v2 second vertex of the current face 111 | * @param v3 third vertex of the current face 112 | * 113 | * @return true if successful; else false 114 | */ 115 | inline bool waveFrontUpdate(lvr2::DenseVertexMap& distances, 116 | lvr2::DenseVertexMap& predecessors, const float& max_distance, 117 | const lvr2::DenseEdgeMap& edge_weights, const lvr2::FaceHandle& fh, 118 | const lvr2::BaseVector& normal, const lvr2::VertexHandle& v1, 119 | const lvr2::VertexHandle& v2, const lvr2::VertexHandle& v3); 120 | 121 | /** 122 | * @brief fade cost value based on lethal and inscribed area 123 | * 124 | * @param val input cost value 125 | * 126 | * @return resulting cost value 127 | */ 128 | float fading(const float val); 129 | 130 | /** 131 | * @brief inflate around lethal vertices by using an wave front propagation and assign riskiness values to vertices 132 | * 133 | * @param lethals set of current lethal vertices 134 | * @param inflation_radius radius of inflation 135 | * @param inscribed_radius rarius of inscribed area 136 | * @param inscribed_value value assigned to inscribed vertices 137 | * @param lethal_value value of lethal vertices 138 | */ 139 | void waveCostInflation(const std::set& lethals, const float inflation_radius, 140 | const float inscribed_radius, const float inscribed_value, const float lethal_value); 141 | 142 | /** 143 | * @brief returns repulsive vector at a given position inside a face 144 | * 145 | * @param vertices vertices of the face 146 | * @param barycentric_coords barycentric coordinates of the requested point inside the face 147 | * 148 | * @return vector of the current vectorfield 149 | */ 150 | lvr2::BaseVector vectorAt(const std::array& vertices, 151 | const std::array& barycentric_coords); 152 | 153 | /** 154 | * @brief returns the repulsive vector at a given vertex 155 | * 156 | * @param vertex requested vertex 157 | * 158 | * @return vector of vectorfield at requested vertex 159 | */ 160 | lvr2::BaseVector vectorAt(const lvr2::VertexHandle& vertex); 161 | 162 | /** 163 | * @brief delivers the current repulsive vectorfield 164 | * 165 | * @return repulsive vectorfield 166 | */ 167 | const boost::optional>&> vectorMap() 168 | { 169 | return vector_map_; 170 | } 171 | 172 | /** 173 | * @brief adds vector pointing back to source of inflation source 174 | * 175 | * @param current_vertex vertex to calculate vector for 176 | * @param predecessors current predecessor map 177 | * @param[out] vector_map resulting vectorfield 178 | */ 179 | void backToSource(const lvr2::VertexHandle& current_vertex, 180 | const lvr2::DenseVertexMap& predecessors, 181 | lvr2::DenseVertexMap>& vector_map); 182 | 183 | /** 184 | * @brief calculate the values of this layer 185 | * 186 | * @return true if successfull; else false 187 | */ 188 | virtual bool computeLayer() override; 189 | 190 | /** 191 | * @brief deliver the current costmap 192 | * 193 | * @return calculated costmap 194 | */ 195 | virtual lvr2::VertexMap& costs() override; 196 | 197 | /** 198 | * @brief deliver set containing all vertices marked as lethal 199 | * 200 | * @return lethal vertices 201 | */ 202 | virtual std::set& lethals() override 203 | { 204 | return lethal_vertices_; 205 | } // TODO remove... layer types 206 | 207 | /** 208 | * @brief update set of lethal vertices by adding and removing vertices 209 | * 210 | * @param added_lethal vertices to be marked as lethal 211 | * @param removed_lethal vertices to be removed from the set of lethal vertices 212 | */ 213 | virtual void updateLethal(std::set& added_lethal, std::set& removed_lethal); 214 | 215 | /** 216 | * @brief initializes this layer plugin 217 | * 218 | * @return true if initialization was successfull; else false 219 | */ 220 | virtual bool initialize() override; 221 | 222 | /** 223 | * @brief callback for incoming param changes 224 | */ 225 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 226 | 227 | lvr2::DenseVertexMap riskiness_; 228 | 229 | lvr2::DenseVertexMap direction_; 230 | 231 | lvr2::DenseVertexMap cutting_faces_; 232 | 233 | lvr2::DenseVertexMap> vector_map_; 234 | 235 | lvr2::DenseVertexMap distances_; 236 | 237 | std::set lethal_vertices_; 238 | 239 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 240 | struct { 241 | double inscribed_radius = 0.25; 242 | double inflation_radius = 0.4; 243 | double factor = 1.0; 244 | double lethal_value = 2.0; 245 | double inscribed_value = 1.0; 246 | int min_contour_size = 3; 247 | bool repulsive_field = true; 248 | } config_; 249 | }; 250 | 251 | } /* namespace mesh_layers */ 252 | 253 | #endif // MESH_MAP__INFLATION_LAYER_H 254 | -------------------------------------------------------------------------------- /mesh_layers/include/mesh_layers/ridge_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, The authors 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 | * authors: 34 | * Malte kl. Piening 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__RIDGE_LAYER_H 39 | #define MESH_MAP__RIDGE_LAYER_H 40 | 41 | #include 42 | #include 43 | 44 | namespace mesh_layers 45 | { 46 | /** 47 | * @brief Costmap layer which assigns high costs to ridges. This is useful for dam avoidance in agricultural 48 | * applications 49 | */ 50 | class RidgeLayer : public mesh_map::AbstractLayer 51 | { 52 | /** 53 | * @brief try read layer from map file 54 | * 55 | * @return true if successul; else false 56 | */ 57 | virtual bool readLayer() override; 58 | 59 | /** 60 | * @brief try to write layer to map file 61 | * 62 | * @return true if successfull; else false 63 | */ 64 | virtual bool writeLayer() override; 65 | 66 | /** 67 | * @brief delivers the threshold above which vertices are marked lethal 68 | * 69 | * @return lethal threshold 70 | */ 71 | virtual float threshold() override; 72 | 73 | /** 74 | * @brief delivers the default layer value 75 | * 76 | * @return default value used for this layer 77 | */ 78 | virtual float defaultValue() override 79 | { 80 | return std::numeric_limits::infinity(); 81 | } 82 | 83 | /** 84 | * @brief calculate the values of this layer 85 | * 86 | * @return true if successfull; else false 87 | */ 88 | virtual bool computeLayer() override; 89 | 90 | /** 91 | * @brief mark vertices with values above the threshold as lethal 92 | * 93 | * @return true if successfull; else false 94 | */ 95 | bool computeLethals(); 96 | 97 | /** 98 | * @brief deliver the current costmap 99 | * 100 | * @return calculated costmap 101 | */ 102 | virtual lvr2::VertexMap& costs() override; 103 | 104 | /** 105 | * @brief deliver set containing all vertices marked as lethal 106 | * 107 | * @return lethal vertices 108 | */ 109 | virtual std::set& lethals() override 110 | { 111 | return lethal_vertices_; 112 | } 113 | 114 | /** 115 | * @brief update set of lethal vertices by adding and removing vertices 116 | * 117 | * @param added_lethal vertices to be marked as lethal 118 | * @param removed_lethal vertices to be removed from the set of lethal vertices 119 | */ 120 | virtual void updateLethal(std::set& added_lethal, std::set& removed_lethal) override {}; 121 | 122 | /** 123 | * @brief initializes this layer plugin 124 | * 125 | * @param name name of this plugin 126 | * 127 | * @return true if initialization was successfull; else false 128 | */ 129 | virtual bool initialize() override; 130 | 131 | /** 132 | * @brief callback for incoming param changes 133 | */ 134 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 135 | 136 | // costmap 137 | lvr2::DenseVertexMap ridge_; 138 | // set of lethal vertices 139 | std::set lethal_vertices_; 140 | 141 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 142 | struct { 143 | double threshold = 0.3; 144 | double radius = 0.3; 145 | double factor = 1.0; 146 | } config_; 147 | }; 148 | 149 | } /* namespace mesh_layers */ 150 | 151 | #endif // MESH_MAP__RIDGE_LAYER_H 152 | -------------------------------------------------------------------------------- /mesh_layers/include/mesh_layers/roughness_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__ROUGHNESS_LAYER_H 39 | #define MESH_MAP__ROUGHNESS_LAYER_H 40 | 41 | #include 42 | #include 43 | 44 | namespace mesh_layers 45 | { 46 | /** 47 | * @brief Costmap layer which calculates the roughness of the surface. This is useful to differentiate paths from 48 | * overgrown areas 49 | */ 50 | class RoughnessLayer : public mesh_map::AbstractLayer 51 | { 52 | /** 53 | * @brief try read layer from map file 54 | * 55 | * @return true if successul; else false 56 | */ 57 | virtual bool readLayer() override; 58 | 59 | /** 60 | * @brief try to write layer to map file 61 | * 62 | * @return true if successfull; else false 63 | */ 64 | virtual bool writeLayer() override; 65 | 66 | /** 67 | * @brief delivers the threshold above which vertices are marked lethal 68 | * 69 | * @return lethal threshold 70 | */ 71 | virtual float threshold() override; 72 | 73 | /** 74 | * @brief delivers the default layer value 75 | * 76 | * @return default value used for this layer 77 | */ 78 | virtual float defaultValue() override 79 | { 80 | return std::numeric_limits::infinity(); 81 | } 82 | 83 | /** 84 | * @brief calculate the values of this layer 85 | * 86 | * @return true if successfull; else false 87 | */ 88 | virtual bool computeLayer() override; 89 | 90 | /** 91 | * @brief mark vertices with values above the threshold as lethal 92 | * 93 | * @return true if successfull; else false 94 | */ 95 | bool computeLethals(); 96 | 97 | /** 98 | * @brief deliver the current costmap 99 | * 100 | * @return calculated costmap 101 | */ 102 | virtual lvr2::VertexMap& costs() override; 103 | 104 | /** 105 | * @brief deliver set containing all vertices marked as lethal 106 | * 107 | * @return lethal vertices 108 | */ 109 | virtual std::set& lethals() override 110 | { 111 | return lethal_vertices_; 112 | } 113 | 114 | /** 115 | * @brief update set of lethal vertices by adding and removing vertices 116 | * 117 | * @param added_lethal vertices to be marked as lethal 118 | * @param removed_lethal vertices to be removed from the set of lethal vertices 119 | */ 120 | virtual void updateLethal(std::set& added_lethal, std::set& removed_lethal) override {}; 121 | 122 | /** 123 | * @brief initializes this layer plugin 124 | * 125 | * @return true if initialization was successfull; else false 126 | */ 127 | virtual bool initialize() override; 128 | 129 | /** 130 | * @brief callback for incoming param changes 131 | */ 132 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 133 | 134 | // latest costmap 135 | lvr2::DenseVertexMap roughness_; 136 | // set of all current lethal vertices 137 | std::set lethal_vertices_; 138 | 139 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 140 | struct { 141 | double threshold = 0.3; 142 | double radius = 0.3; 143 | double factor = 1.0; 144 | } config_; 145 | }; 146 | 147 | } /* namespace mesh_layers */ 148 | 149 | #endif // MESH_MAP__ROUGHNESS_LAYER_H 150 | -------------------------------------------------------------------------------- /mesh_layers/include/mesh_layers/steepness_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, The authors 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 | * authors: 34 | * Malte kl. Piening 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__STEEPNESS_LAYER_H 39 | #define MESH_MAP__STEEPNESS_LAYER_H 40 | 41 | #include 42 | #include 43 | 44 | namespace mesh_layers 45 | { 46 | /** 47 | * @brief Costmap layer which calculates the steepness of the vertices. This is useful to avoid stairs 48 | */ 49 | class SteepnessLayer : public mesh_map::AbstractLayer 50 | { 51 | /** 52 | * @brief try read layer from map file 53 | * 54 | * @return true if successul; else false 55 | */ 56 | virtual bool readLayer() override; 57 | 58 | /** 59 | * @brief try to write layer to map file 60 | * 61 | * @return true if successfull; else false 62 | */ 63 | virtual bool writeLayer() override; 64 | 65 | /** 66 | * @brief delivers the threshold above which vertices are marked lethal 67 | * 68 | * @return lethal threshold 69 | */ 70 | virtual float threshold() override; 71 | 72 | /** 73 | * @brief delivers the default layer value 74 | * 75 | * @return default value used for this layer 76 | */ 77 | virtual float defaultValue() override 78 | { 79 | return std::numeric_limits::infinity(); 80 | } 81 | 82 | /** 83 | * @brief calculate the values of this layer 84 | * 85 | * @return true if successfull; else false 86 | */ 87 | virtual bool computeLayer() override; 88 | 89 | /** 90 | * @brief mark vertices with values above the threshold as lethal 91 | * 92 | * @return true if successfull; else false 93 | */ 94 | bool computeLethals(); 95 | 96 | /** 97 | * @brief deliver the current costmap 98 | * 99 | * @return calculated costmap 100 | */ 101 | virtual lvr2::VertexMap& costs() override; 102 | 103 | /** 104 | * @brief deliver set containing all vertices marked as lethal 105 | * 106 | * @return lethal vertices 107 | */ 108 | virtual std::set& lethals() override 109 | { 110 | return lethal_vertices_; 111 | } 112 | 113 | /** 114 | * @brief update set of lethal vertices by adding and removing vertices 115 | * 116 | * @param added_lethal vertices to be marked as lethal 117 | * @param removed_lethal vertices to be removed from the set of lethal vertices 118 | */ 119 | virtual void updateLethal(std::set& added_lethal, std::set& removed_lethal) override {}; 120 | 121 | /** 122 | * @brief initializes this layer plugin 123 | * 124 | * @return true if initialization was successfull; else false 125 | */ 126 | virtual bool initialize() override; 127 | 128 | /** 129 | * @brief callback for incoming param changes 130 | */ 131 | rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); 132 | 133 | // latest costmap 134 | lvr2::DenseVertexMap steepness_; 135 | // set of all current lethal vertices 136 | std::set lethal_vertices_; 137 | 138 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 139 | struct { 140 | double threshold = 0.3; 141 | double factor = 1.0; 142 | } config_; 143 | 144 | }; 145 | 146 | } /* namespace mesh_layers */ 147 | 148 | #endif // MESH_MAP__STEEPNESS_LAYER_H 149 | -------------------------------------------------------------------------------- /mesh_layers/mesh_layers.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A border layer, setting the border vertices of a mesh to high costs. Use this layer to prohibit the robot to navigate to the border of the mesh. 7 | 8 | 9 | 12 | 13 | A roughness mesh layer, computing the roughness of the surface as cost layer. 14 | 15 | 16 | 19 | 20 | A height differences mesh layer, computing the height differences of the surface as cost layer. 21 | 22 | 23 | 26 | 27 | A inflation mesh layer, computing the inflation of the surface as cost layer. 28 | 29 | 30 | 33 | 34 | Calculates the steepness of the surface as a cost layer. 35 | 36 | 37 | 40 | 41 | Calculates an indicator whether or not a vertex is part of a ridge. 42 | 43 | 44 | 47 | 48 | Calculates the free space along the vertex normals and uses it to check if the robot fits through a space. Useful for doors, holes etc. 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /mesh_layers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_layers 4 | 2.0.0 5 | The mesh_layers package 6 | 7 | Matthias Holoch 8 | Sebastian Pütz 9 | 10 | BSD-3 11 | Sebastian Pütz 12 | 13 | mesh_map 14 | lvr2 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /mesh_layers/src/border_layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, Alexander Mock 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 | * authors: 34 | * Alexander Mock 35 | * 36 | */ 37 | 38 | #include "mesh_layers/border_layer.h" 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(mesh_layers::BorderLayer, mesh_map::AbstractLayer) 45 | 46 | namespace mesh_layers 47 | { 48 | bool BorderLayer::readLayer() 49 | { 50 | RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read border costs from map file..."); 51 | auto mesh_io = map_ptr_->meshIO(); 52 | auto border_costs_opt = mesh_io->getDenseAttributeMap >(layer_name_); 53 | 54 | if (border_costs_opt) 55 | { 56 | RCLCPP_INFO_STREAM(node_->get_logger(), "Border costs have been read successfully."); 57 | border_costs_ = border_costs_opt.get(); 58 | return computeLethals(); 59 | } 60 | 61 | return false; 62 | } 63 | 64 | bool BorderLayer::computeLethals() 65 | { 66 | RCLCPP_INFO_STREAM(node_->get_logger(), "Compute lethals for \"" << layer_name_ << "\" (Border Layer) with threshold " 67 | << config_.threshold); 68 | lethal_vertices_.clear(); 69 | for (auto vH : border_costs_) 70 | { 71 | if (border_costs_[vH] > config_.threshold) 72 | { 73 | lethal_vertices_.insert(vH); 74 | } 75 | } 76 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << lethal_vertices_.size() << " lethal vertices."); 77 | return true; 78 | } 79 | 80 | bool BorderLayer::writeLayer() 81 | { 82 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saving border costs to map file..."); 83 | auto mesh_io = map_ptr_->meshIO(); 84 | if (mesh_io->addDenseAttributeMap(border_costs_, layer_name_)) 85 | { 86 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved border costs to map file."); 87 | return true; 88 | } 89 | else 90 | { 91 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save height differences to map file!"); 92 | return false; 93 | } 94 | } 95 | 96 | float BorderLayer::threshold() 97 | { 98 | return config_.threshold; 99 | } 100 | 101 | bool BorderLayer::computeLayer() 102 | { 103 | border_costs_ = lvr2::calcBorderCosts(*(map_ptr_->mesh()), config_.border_cost); 104 | return computeLethals(); 105 | } 106 | 107 | lvr2::VertexMap& BorderLayer::costs() 108 | { 109 | return border_costs_; 110 | } 111 | 112 | rcl_interfaces::msg::SetParametersResult BorderLayer::reconfigureCallback(std::vector parameters) 113 | { 114 | rcl_interfaces::msg::SetParametersResult result; 115 | result.successful = true; 116 | 117 | // bool has_threshold_changed = false; 118 | bool recompute_costs = false; 119 | bool recompute_lethals = false; 120 | 121 | for (auto parameter : parameters) 122 | { 123 | if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") { 124 | config_.threshold = parameter.as_double(); 125 | recompute_lethals = true; 126 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost") { 127 | config_.border_cost = parameter.as_double(); 128 | recompute_costs = true; 129 | recompute_lethals = true; 130 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { 131 | config_.factor = parameter.as_double(); 132 | } 133 | } 134 | 135 | if(recompute_costs) 136 | { 137 | RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Recompute layer costs due to cfg change."); 138 | computeLayer(); 139 | } 140 | 141 | if (recompute_lethals) 142 | { 143 | RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Recompute lethals due to cfg change."); 144 | computeLethals(); 145 | } 146 | 147 | if(recompute_costs || recompute_lethals) 148 | { 149 | RCLCPP_INFO_STREAM(node_->get_logger(), "'" << layer_name_ << "': Notify changes."); 150 | notifyChange(); 151 | } 152 | 153 | return result; 154 | } 155 | 156 | bool BorderLayer::initialize() 157 | { 158 | { // threshold 159 | rcl_interfaces::msg::ParameterDescriptor descriptor; 160 | descriptor.description = "Threshold for the local border costs to be counted as lethal."; 161 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 162 | rcl_interfaces::msg::FloatingPointRange range; 163 | range.from_value = 0.05; 164 | range.to_value = 1.0; 165 | descriptor.floating_point_range.push_back(range); 166 | config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); 167 | } 168 | { // border cost 169 | rcl_interfaces::msg::ParameterDescriptor descriptor; 170 | descriptor.description = "The cost value used for the border vertices."; 171 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 172 | rcl_interfaces::msg::FloatingPointRange range; 173 | range.from_value = 0.02; 174 | range.to_value = 10.0; 175 | descriptor.floating_point_range.push_back(range); 176 | config_.border_cost = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".border_cost", config_.border_cost, descriptor); 177 | } 178 | { // factor 179 | rcl_interfaces::msg::ParameterDescriptor descriptor; 180 | descriptor.description = "Using this factor to weight this layer."; 181 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 182 | rcl_interfaces::msg::FloatingPointRange range; 183 | range.from_value = 0.0; 184 | range.to_value = 1.0; 185 | descriptor.floating_point_range.push_back(range); 186 | config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); 187 | } 188 | dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( 189 | &BorderLayer::reconfigureCallback, this, std::placeholders::_1)); 190 | return true; 191 | } 192 | 193 | } /* namespace mesh_layers */ 194 | -------------------------------------------------------------------------------- /mesh_layers/src/clearance_layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2024, Justus Braun 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 | * authors: 34 | * Justus Braun 35 | * 36 | */ 37 | 38 | #include "mesh_layers/clearance_layer.h" 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(mesh_layers::ClearanceLayer, mesh_map::AbstractLayer) 45 | 46 | namespace mesh_layers 47 | { 48 | bool ClearanceLayer::readLayer() 49 | { 50 | RCLCPP_INFO(node_->get_logger(), "Try to read '%s' from map file...", layer_name_.c_str()); 51 | auto clearance_opt = map_ptr_->meshIO()->getDenseAttributeMap>(layer_name_); 52 | 53 | if (clearance_opt) 54 | { 55 | RCLCPP_INFO(node_->get_logger(), "'%s' has been read successfully.", layer_name_.c_str()); 56 | clearance_ = clearance_opt.get(); 57 | 58 | return computeLethalsAndCosts(); 59 | } 60 | 61 | return false; 62 | } 63 | 64 | bool ClearanceLayer::computeLethalsAndCosts() 65 | { 66 | RCLCPP_INFO_STREAM(node_->get_logger(), "Compute lethals for '" << layer_name_ << "' (Clearance Layer) with robot_height " << config_.robot_height << " and height_inflation " << config_.height_inflation); 67 | lethal_vertices_.clear(); 68 | costs_.clear(); 69 | 70 | // The clearance_ map contains the actual free space above the vertices. 71 | // If the robot fits through a gap with the height inflation the cost is 0. 72 | // If the robot does not fit the cost is 1. 73 | // Between robot_height and robot_height + height_inflation the cost decreases. 74 | const double inflated_height = config_.robot_height + config_.height_inflation; 75 | for (auto vH : clearance_) 76 | { 77 | if (clearance_[vH] < config_.robot_height) 78 | { 79 | costs_.insert(vH, 1.0); 80 | lethal_vertices_.insert(vH); 81 | } 82 | else if (clearance_[vH] < inflated_height) 83 | { 84 | // Map the cost to [1, 0] using (cos(x * pi) + 1 / 2) 85 | const double diff = (clearance_[vH] - config_.robot_height) / config_.height_inflation; 86 | const double cost = (cos(diff * M_PI) + 1.0) / 2.0; 87 | costs_.insert(vH, cost); 88 | } 89 | else 90 | { 91 | costs_.insert(vH, 0.0); 92 | } 93 | } 94 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << lethal_vertices_.size() << " lethal vertices."); 95 | return true; 96 | } 97 | 98 | bool ClearanceLayer::writeLayer() 99 | { 100 | RCLCPP_INFO(node_->get_logger(), "Saving '%s' to map file...", layer_name_.c_str()); 101 | if (map_ptr_->meshIO()->addDenseAttributeMap(clearance_, layer_name_)) 102 | { 103 | RCLCPP_INFO(node_->get_logger(), "Saved '%s' to map file.", layer_name_.c_str()); 104 | return true; 105 | } 106 | else 107 | { 108 | RCLCPP_ERROR(node_->get_logger(), "Could not save '%s' to map file!", layer_name_.c_str()); 109 | return false; 110 | } 111 | } 112 | 113 | float ClearanceLayer::threshold() 114 | { 115 | return config_.robot_height; 116 | } 117 | 118 | bool ClearanceLayer::computeLayer() 119 | { 120 | RCLCPP_INFO(node_->get_logger(), "Computing clearance along vertex normals..."); 121 | 122 | // Load vertex normals 123 | using VertexNormalMap = lvr2::DenseVertexMap; 124 | VertexNormalMap vertex_normals; 125 | auto vertex_normals_opt = map_ptr_->meshIO()->getDenseAttributeMap("vertex_normals"); 126 | 127 | if (vertex_normals_opt) 128 | { 129 | RCLCPP_INFO(node_->get_logger(), "Using vertex normals from map file"); 130 | vertex_normals = vertex_normals_opt.value(); 131 | } 132 | else 133 | { 134 | RCLCPP_INFO(node_->get_logger(), "No vertex normals in map file!"); 135 | 136 | // Load or calculate face normals 137 | using FaceNormalMap = lvr2::DenseFaceMap; 138 | FaceNormalMap face_normals; 139 | auto face_normals_opt = map_ptr_->meshIO()->getDenseAttributeMap("face_normals"); 140 | if (face_normals_opt) 141 | { 142 | RCLCPP_INFO(node_->get_logger(), "Using face normals from map file"); 143 | face_normals = face_normals_opt.value(); 144 | } 145 | else 146 | { 147 | RCLCPP_INFO(node_->get_logger(), "Calculating face normals"); 148 | face_normals = lvr2::calcFaceNormals(*map_ptr_->mesh()); 149 | } 150 | 151 | RCLCPP_INFO(node_->get_logger(), "Calculating vertex normals"); 152 | VertexNormalMap vertex_normals = lvr2::calcVertexNormals(*map_ptr_->mesh(), face_normals); 153 | } 154 | 155 | // Finally calculate the clearance layer 156 | clearance_ = lvr2::calcNormalClearance(*map_ptr_->mesh(), vertex_normals); 157 | 158 | return computeLethalsAndCosts(); 159 | } 160 | 161 | lvr2::VertexMap& ClearanceLayer::costs() 162 | { 163 | return costs_; 164 | } 165 | 166 | rcl_interfaces::msg::SetParametersResult ClearanceLayer::reconfigureCallback(std::vector parameters) 167 | { 168 | rcl_interfaces::msg::SetParametersResult result; 169 | result.successful = true; 170 | 171 | bool needs_cost_recompute = false; 172 | for (auto parameter : parameters) { 173 | if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".robot_height") { 174 | config_.robot_height = parameter.as_double(); 175 | needs_cost_recompute = true; 176 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".height_inflation") { 177 | config_.height_inflation = parameter.as_double(); 178 | needs_cost_recompute = true; 179 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { 180 | config_.factor = parameter.as_double(); 181 | } 182 | } 183 | 184 | if (needs_cost_recompute) { 185 | RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute costs and notify change from " << layer_name_ << " due to cfg change."); 186 | computeLethalsAndCosts(); 187 | notifyChange(); 188 | } 189 | 190 | return result; 191 | } 192 | 193 | 194 | bool ClearanceLayer::initialize() 195 | { 196 | { // threshold 197 | rcl_interfaces::msg::ParameterDescriptor descriptor; 198 | descriptor.description = "Threshold for the robot to fit through the space in meters (height of the robot plus margin)."; 199 | rcl_interfaces::msg::FloatingPointRange range; 200 | range.from_value = 0.05; 201 | range.to_value = 100.0; // What is a sane limit for robot size? 202 | descriptor.floating_point_range.push_back(range); 203 | config_.robot_height = node_->declare_parameter( 204 | mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".robot_height", 205 | descriptor 206 | ); 207 | } 208 | { // Extra clearance for the robot to avoid tight spaces 209 | rcl_interfaces::msg::ParameterDescriptor descriptor; 210 | descriptor.description = "Extra headroom for the robot in which the cost decreases from 1 to 0."; 211 | config_.height_inflation = node_->declare_parameter( 212 | mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".height_inflation", 213 | config_.height_inflation, 214 | descriptor 215 | ); 216 | } 217 | { // factor 218 | rcl_interfaces::msg::ParameterDescriptor descriptor; 219 | descriptor.description = "Using this factor to weight this layer."; 220 | rcl_interfaces::msg::FloatingPointRange range; 221 | range.from_value = 0.0; 222 | range.to_value = 1.0; 223 | descriptor.floating_point_range.push_back(range); 224 | config_.factor = node_->declare_parameter( 225 | mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", 226 | config_.factor, 227 | descriptor 228 | ); 229 | } 230 | dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( 231 | &ClearanceLayer::reconfigureCallback, this, std::placeholders::_1)); 232 | return true; 233 | } 234 | 235 | } /* namespace mesh_layers */ 236 | -------------------------------------------------------------------------------- /mesh_layers/src/height_diff_layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #include "mesh_layers/height_diff_layer.h" 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(mesh_layers::HeightDiffLayer, mesh_map::AbstractLayer) 45 | 46 | namespace mesh_layers 47 | { 48 | bool HeightDiffLayer::readLayer() 49 | { 50 | RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read height differences from map file..."); 51 | auto mesh_io = map_ptr_->meshIO(); 52 | auto height_diff_opt = mesh_io->getDenseAttributeMap>(layer_name_); 53 | 54 | if (height_diff_opt) 55 | { 56 | RCLCPP_INFO_STREAM(node_->get_logger(), "Height differences have been read successfully."); 57 | height_diff_ = height_diff_opt.get(); 58 | 59 | return computeLethals(); 60 | } 61 | 62 | return false; 63 | } 64 | 65 | bool HeightDiffLayer::computeLethals() 66 | { 67 | RCLCPP_INFO_STREAM(node_->get_logger(), "Compute lethals for \"" << layer_name_ << "\" (Height Differences Layer) with threshold " 68 | << config_.threshold); 69 | lethal_vertices_.clear(); 70 | for (auto vH : height_diff_) 71 | { 72 | if (height_diff_[vH] > config_.threshold) 73 | lethal_vertices_.insert(vH); 74 | } 75 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << lethal_vertices_.size() << " lethal vertices."); 76 | return true; 77 | } 78 | 79 | bool HeightDiffLayer::writeLayer() 80 | { 81 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saving height_differences to map file..."); 82 | auto mesh_io = map_ptr_->meshIO(); 83 | if (mesh_io->addDenseAttributeMap(height_diff_, layer_name_)) 84 | { 85 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved height differences to map file."); 86 | return true; 87 | } 88 | else 89 | { 90 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save height differences to map file!"); 91 | return false; 92 | } 93 | } 94 | 95 | float HeightDiffLayer::threshold() 96 | { 97 | return config_.threshold; 98 | } 99 | 100 | bool HeightDiffLayer::computeLayer() 101 | { 102 | auto mesh = map_ptr_->mesh(); 103 | height_diff_ = lvr2::calcVertexHeightDifferences(*mesh, config_.radius); 104 | return computeLethals(); 105 | } 106 | 107 | lvr2::VertexMap& HeightDiffLayer::costs() 108 | { 109 | return height_diff_; 110 | } 111 | 112 | rcl_interfaces::msg::SetParametersResult HeightDiffLayer::reconfigureCallback(std::vector parameters) 113 | { 114 | rcl_interfaces::msg::SetParametersResult result; 115 | result.successful = true; 116 | 117 | bool recompute_lethals = false; 118 | for (auto parameter : parameters) { 119 | if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") { 120 | config_.threshold = parameter.as_double(); 121 | recompute_lethals = true; 122 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") { 123 | config_.radius = parameter.as_double(); 124 | recompute_lethals = true; 125 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { 126 | config_.factor = parameter.as_double(); 127 | } 128 | } 129 | 130 | if (recompute_lethals) { 131 | RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute lethals and notify change from " << layer_name_ << " due to cfg change."); 132 | computeLethals(); 133 | notifyChange(); 134 | } 135 | 136 | return result; 137 | } 138 | 139 | 140 | bool HeightDiffLayer::initialize() 141 | { 142 | { // threshold 143 | rcl_interfaces::msg::ParameterDescriptor descriptor; 144 | descriptor.description = "Threshold for the local height difference to be counted as lethal."; 145 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 146 | rcl_interfaces::msg::FloatingPointRange range; 147 | range.from_value = 0.05; 148 | range.to_value = 1.0; 149 | descriptor.floating_point_range.push_back(range); 150 | config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); 151 | } 152 | { // radius 153 | rcl_interfaces::msg::ParameterDescriptor descriptor; 154 | descriptor.description = "The radius used for calculating the local height difference."; 155 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 156 | rcl_interfaces::msg::FloatingPointRange range; 157 | range.from_value = 0.02; 158 | range.to_value = 1.0; 159 | descriptor.floating_point_range.push_back(range); 160 | config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor); 161 | } 162 | { // factor 163 | rcl_interfaces::msg::ParameterDescriptor descriptor; 164 | descriptor.description = "Using this factor to weight this layer."; 165 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 166 | rcl_interfaces::msg::FloatingPointRange range; 167 | range.from_value = 0.0; 168 | range.to_value = 1.0; 169 | descriptor.floating_point_range.push_back(range); 170 | config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); 171 | } 172 | dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( 173 | &HeightDiffLayer::reconfigureCallback, this, std::placeholders::_1)); 174 | return true; 175 | } 176 | 177 | } /* namespace mesh_layers */ 178 | -------------------------------------------------------------------------------- /mesh_layers/src/ridge_layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, The authors 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 | * authors: 34 | * Malte kl. Piening 35 | * 36 | */ 37 | 38 | #include "mesh_layers/ridge_layer.h" 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | PLUGINLIB_EXPORT_CLASS(mesh_layers::RidgeLayer, mesh_map::AbstractLayer) 47 | 48 | namespace mesh_layers 49 | { 50 | bool RidgeLayer::readLayer() 51 | { 52 | RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read ridge from map file..."); 53 | auto mesh_io = map_ptr_->meshIO(); 54 | auto ridge_opt = mesh_io->getDenseAttributeMap>(layer_name_); 55 | if (ridge_opt) 56 | { 57 | RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read ridge from map file."); 58 | ridge_ = ridge_opt.get(); 59 | return computeLethals(); 60 | } 61 | 62 | return false; 63 | } 64 | 65 | bool RidgeLayer::writeLayer() 66 | { 67 | auto mesh_io = map_ptr_->meshIO(); 68 | if (mesh_io->addDenseAttributeMap(ridge_, layer_name_)) 69 | { 70 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved ridge to map file."); 71 | return true; 72 | } 73 | else 74 | { 75 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save ridge to map file!"); 76 | return false; 77 | } 78 | } 79 | 80 | bool RidgeLayer::computeLethals() 81 | { 82 | RCLCPP_INFO_STREAM(node_->get_logger(), "Compute lethals for \"" << layer_name_ << "\" (Ridge Layer) with threshold " << config_.threshold); 83 | lethal_vertices_.clear(); 84 | for (auto vH : ridge_) 85 | { 86 | if (ridge_[vH] > config_.threshold) 87 | lethal_vertices_.insert(vH); 88 | } 89 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << lethal_vertices_.size() << " lethal vertices."); 90 | return true; 91 | } 92 | 93 | float RidgeLayer::threshold() 94 | { 95 | return config_.threshold; 96 | } 97 | 98 | bool RidgeLayer::computeLayer() 99 | { 100 | RCLCPP_INFO_STREAM(node_->get_logger(), "Computing ridge..."); 101 | 102 | lvr2::DenseFaceMap face_normals; 103 | 104 | const auto mesh = map_ptr_->mesh(); 105 | auto mesh_io = map_ptr_->meshIO(); 106 | auto face_normals_opt = mesh_io->getDenseAttributeMap>("face_normals"); 107 | 108 | if (face_normals_opt) 109 | { 110 | face_normals = face_normals_opt.get(); 111 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << face_normals.numValues() << " face normals in map file."); 112 | } 113 | else 114 | { 115 | RCLCPP_INFO_STREAM(node_->get_logger(), "No face normals found in the given map file, computing them..."); 116 | face_normals = lvr2::calcFaceNormals(*mesh); 117 | RCLCPP_INFO_STREAM(node_->get_logger(), "Computed " << face_normals.numValues() << " face normals."); 118 | if (mesh_io->addDenseAttributeMap(face_normals, "face_normals")) 119 | { 120 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved face normals to map file."); 121 | } 122 | else 123 | { 124 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save face normals to map file!"); 125 | return false; 126 | } 127 | } 128 | 129 | lvr2::DenseVertexMap vertex_normals; 130 | auto vertex_normals_opt = mesh_io->getDenseAttributeMap>("vertex_normals"); 131 | 132 | if (vertex_normals_opt) 133 | { 134 | vertex_normals = vertex_normals_opt.get(); 135 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << vertex_normals.numValues() << " vertex normals in map file!"); 136 | } 137 | else 138 | { 139 | RCLCPP_INFO_STREAM(node_->get_logger(), "No vertex normals found in the given map file, computing them..."); 140 | vertex_normals = lvr2::calcVertexNormals(*mesh, face_normals); 141 | if (mesh_io->addDenseAttributeMap(vertex_normals, "vertex_normals")) 142 | { 143 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved vertex normals to map file."); 144 | } 145 | else 146 | { 147 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save vertex normals to map file!"); 148 | return false; 149 | } 150 | } 151 | 152 | ridge_.reserve(mesh->nextVertexIndex()); 153 | 154 | for (size_t i = 0; i < mesh->nextVertexIndex(); i++) 155 | { 156 | auto vH = lvr2::VertexHandle(i); 157 | if (!mesh->containsVertex(vH)) 158 | { 159 | ridge_.insert(vH, config_.threshold + 0.1); 160 | continue; 161 | } 162 | 163 | std::set invalid; 164 | 165 | float value = 0.0; 166 | int num_neighbours = 0; 167 | lvr2::BaseVector reference = mesh->getVertexPosition(vH) + vertex_normals[vH]; 168 | visitLocalVertexNeighborhood(*mesh, invalid, vH, config_.radius, [&](auto vertex) { 169 | lvr2::BaseVector current_point = mesh->getVertexPosition(vertex) + vertex_normals[vertex]; 170 | value += (current_point - reference).length(); 171 | num_neighbours++; 172 | }); 173 | 174 | if (num_neighbours == 0) 175 | { 176 | ridge_.insert(vH, config_.threshold + 0.1); 177 | continue; 178 | } 179 | 180 | ridge_.insert(vH, value / num_neighbours); 181 | } 182 | 183 | return computeLethals(); 184 | } 185 | 186 | lvr2::VertexMap& RidgeLayer::costs() 187 | { 188 | return ridge_; 189 | } 190 | 191 | rcl_interfaces::msg::SetParametersResult RidgeLayer::reconfigureCallback(std::vector parameters) 192 | { 193 | rcl_interfaces::msg::SetParametersResult result; 194 | result.successful = true; 195 | bool has_radius_changed = false; 196 | bool has_threshold_changed = false; 197 | 198 | for (auto parameter : parameters) { 199 | if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") { 200 | config_.threshold = parameter.as_double(); 201 | has_threshold_changed = true; 202 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") { 203 | config_.radius = parameter.as_double(); 204 | has_radius_changed = true; 205 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { 206 | config_.factor = parameter.as_double(); 207 | } 208 | } 209 | 210 | if (has_threshold_changed) { computeLethals(); } 211 | if (has_radius_changed) { computeLayer(); } 212 | if (has_radius_changed || has_threshold_changed) 213 | { 214 | RCLCPP_INFO_STREAM(node_->get_logger(), "Notify change from " << layer_name_ << " (RidgeLayer) due to cfg change."); 215 | notifyChange(); 216 | } 217 | 218 | return result; 219 | } 220 | 221 | bool RidgeLayer::initialize() 222 | { 223 | { // threshold 224 | rcl_interfaces::msg::ParameterDescriptor descriptor; 225 | descriptor.description = "Threshold for the local ridge to be counted as lethal."; 226 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 227 | rcl_interfaces::msg::FloatingPointRange range; 228 | range.from_value = 0.01; 229 | range.to_value = 3.1415; 230 | descriptor.floating_point_range.push_back(range); 231 | config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); 232 | } 233 | { // radius 234 | rcl_interfaces::msg::ParameterDescriptor descriptor; 235 | descriptor.description = "Radius of the inscribed area."; 236 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 237 | rcl_interfaces::msg::FloatingPointRange range; 238 | range.from_value = 0.01; 239 | range.to_value = 1.0; 240 | descriptor.floating_point_range.push_back(range); 241 | config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor); 242 | } 243 | { // factor 244 | rcl_interfaces::msg::ParameterDescriptor descriptor; 245 | descriptor.description = "The local ridge factor to weight this layer."; 246 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 247 | rcl_interfaces::msg::FloatingPointRange range; 248 | range.from_value = 0.0; 249 | range.to_value = 1.0; 250 | descriptor.floating_point_range.push_back(range); 251 | config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); 252 | } 253 | dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( 254 | &RidgeLayer::reconfigureCallback, this, std::placeholders::_1)); 255 | return true; 256 | } 257 | 258 | } /* namespace mesh_layers */ 259 | -------------------------------------------------------------------------------- /mesh_layers/src/roughness_layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #include "mesh_layers/roughness_layer.h" 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(mesh_layers::RoughnessLayer, mesh_map::AbstractLayer) 45 | 46 | namespace mesh_layers { 47 | 48 | bool RoughnessLayer::readLayer() { 49 | RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read roughness from map file..."); 50 | auto mesh_io = map_ptr_->meshIO(); 51 | auto roughness_opt = 52 | mesh_io->getDenseAttributeMap>( 53 | layer_name_); 54 | if (roughness_opt) { 55 | RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read roughness from map file."); 56 | roughness_ = roughness_opt.get(); 57 | return computeLethals(); 58 | } 59 | 60 | return false; 61 | } 62 | 63 | bool RoughnessLayer::writeLayer() { 64 | auto mesh_io = map_ptr_->meshIO(); 65 | if (mesh_io->addDenseAttributeMap(roughness_, layer_name_)) { 66 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved roughness to map file."); 67 | return true; 68 | } else { 69 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save roughness to map file!"); 70 | return false; 71 | } 72 | } 73 | 74 | bool RoughnessLayer::computeLethals() 75 | { 76 | RCLCPP_INFO_STREAM(node_->get_logger(), "Compute lethals for \"" << layer_name_ << "\" (Roughness Layer) with threshold " << config_.threshold); 77 | lethal_vertices_.clear(); 78 | for (auto vH : roughness_) { 79 | if (roughness_[vH] > config_.threshold) 80 | lethal_vertices_.insert(vH); 81 | } 82 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << lethal_vertices_.size() << " lethal vertices."); 83 | return true; 84 | } 85 | 86 | float RoughnessLayer::threshold() { return config_.threshold; } 87 | 88 | bool RoughnessLayer::computeLayer() { 89 | RCLCPP_INFO_STREAM(node_->get_logger(), "Computing roughness..."); 90 | 91 | lvr2::DenseFaceMap face_normals; 92 | 93 | const auto mesh = map_ptr_->mesh(); 94 | auto mesh_io = map_ptr_->meshIO(); 95 | 96 | auto face_normals_opt = 97 | mesh_io->getDenseAttributeMap>( 98 | "face_normals"); 99 | 100 | if (face_normals_opt) { 101 | face_normals = face_normals_opt.get(); 102 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << face_normals.numValues() 103 | << " face normals in map file."); 104 | } else { 105 | RCLCPP_INFO_STREAM(node_->get_logger(), 106 | "No face normals found in the given map file, computing them..."); 107 | face_normals = lvr2::calcFaceNormals(*mesh); 108 | RCLCPP_INFO_STREAM(node_->get_logger(), "Computed " << face_normals.numValues() 109 | << " face normals."); 110 | if (mesh_io->addDenseAttributeMap(face_normals, "face_normals")) { 111 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved face normals to map file."); 112 | } else { 113 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save face normals to map file!"); 114 | return false; 115 | } 116 | } 117 | 118 | lvr2::DenseVertexMap vertex_normals; 119 | auto vertex_normals_opt = 120 | mesh_io->getDenseAttributeMap>( 121 | "vertex_normals"); 122 | 123 | if (vertex_normals_opt) { 124 | vertex_normals = vertex_normals_opt.get(); 125 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << vertex_normals.numValues() 126 | << " vertex normals in map file!"); 127 | } else { 128 | RCLCPP_INFO_STREAM(node_->get_logger(), 129 | "No vertex normals found in the given map file, computing them..."); 130 | vertex_normals = lvr2::calcVertexNormals(*mesh, face_normals); 131 | if (mesh_io->addDenseAttributeMap(vertex_normals, "vertex_normals")) { 132 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved vertex normals to map file."); 133 | } else { 134 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save vertex normals to map file!"); 135 | return false; 136 | } 137 | } 138 | 139 | roughness_ = 140 | lvr2::calcVertexRoughness(*mesh, config_.radius, vertex_normals); 141 | 142 | return computeLethals(); 143 | } 144 | 145 | lvr2::VertexMap &RoughnessLayer::costs() { return roughness_; } 146 | 147 | rcl_interfaces::msg::SetParametersResult RoughnessLayer::reconfigureCallback(std::vector parameters) 148 | { 149 | rcl_interfaces::msg::SetParametersResult result; 150 | result.successful = true; 151 | 152 | bool has_threshold_changed = false; 153 | for (auto parameter : parameters) { 154 | if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") { 155 | config_.threshold = parameter.as_double(); 156 | has_threshold_changed = true; 157 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius") { 158 | config_.radius = parameter.as_double(); 159 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { 160 | config_.factor = parameter.as_double(); 161 | } 162 | } 163 | 164 | if (has_threshold_changed) { 165 | RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute lethals and notify change from " << layer_name_ << " due to cfg change."); 166 | computeLethals(); 167 | notifyChange(); 168 | } 169 | 170 | return result; 171 | } 172 | 173 | bool RoughnessLayer::initialize() { 174 | { // threshold 175 | rcl_interfaces::msg::ParameterDescriptor descriptor; 176 | descriptor.description = "Threshold for the local roughness to be counted as lethal."; 177 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 178 | rcl_interfaces::msg::FloatingPointRange range; 179 | range.from_value = 0.01; 180 | range.to_value = 3.1415; 181 | descriptor.floating_point_range.push_back(range); 182 | config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); 183 | } 184 | { // radius 185 | rcl_interfaces::msg::ParameterDescriptor descriptor; 186 | descriptor.description = "The radius used for calculating the local roughness."; 187 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 188 | rcl_interfaces::msg::FloatingPointRange range; 189 | range.from_value = 0.02; 190 | range.to_value = 1.0; 191 | descriptor.floating_point_range.push_back(range); 192 | config_.radius = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".radius", config_.radius, descriptor); 193 | } 194 | { // factor 195 | rcl_interfaces::msg::ParameterDescriptor descriptor; 196 | descriptor.description = "The local roughness factor to weight this layer."; 197 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 198 | rcl_interfaces::msg::FloatingPointRange range; 199 | range.from_value = 0.0; 200 | range.to_value = 1.0; 201 | descriptor.floating_point_range.push_back(range); 202 | config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); 203 | } 204 | dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( 205 | &RoughnessLayer::reconfigureCallback, this, std::placeholders::_1)); 206 | return true; 207 | } 208 | 209 | } /* namespace mesh_layers */ 210 | -------------------------------------------------------------------------------- /mesh_layers/src/steepness_layer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, The authors 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 | * authors: 34 | * Malte kl. Piening 35 | * 36 | */ 37 | 38 | #include "mesh_layers/steepness_layer.h" 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | PLUGINLIB_EXPORT_CLASS(mesh_layers::SteepnessLayer, mesh_map::AbstractLayer) 46 | 47 | namespace mesh_layers 48 | { 49 | bool SteepnessLayer::readLayer() 50 | { 51 | RCLCPP_INFO_STREAM(node_->get_logger(), "Try to read steepness from map file..."); 52 | auto mesh_io = map_ptr_->meshIO(); 53 | auto steepness_opt = mesh_io->getDenseAttributeMap>(layer_name_); 54 | if (steepness_opt) 55 | { 56 | RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully read steepness from map file."); 57 | steepness_ = steepness_opt.get(); 58 | return computeLethals(); 59 | } 60 | 61 | return false; 62 | } 63 | 64 | bool SteepnessLayer::writeLayer() 65 | { 66 | auto mesh_io = map_ptr_->meshIO(); 67 | if (mesh_io->addDenseAttributeMap(steepness_, layer_name_)) 68 | { 69 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved steepness to map file."); 70 | return true; 71 | } 72 | else 73 | { 74 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save steepness to map file!"); 75 | return false; 76 | } 77 | } 78 | 79 | bool SteepnessLayer::computeLethals() 80 | { 81 | RCLCPP_INFO_STREAM(node_->get_logger(), "Compute lethals for \"" << layer_name_ << "\" (Steepness Layer) with threshold " << config_.threshold); 82 | lethal_vertices_.clear(); 83 | for (auto vH : steepness_) 84 | { 85 | if (steepness_[vH] > config_.threshold) 86 | lethal_vertices_.insert(vH); 87 | } 88 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << lethal_vertices_.size() << " lethal vertices."); 89 | return true; 90 | } 91 | 92 | float SteepnessLayer::threshold() 93 | { 94 | return config_.threshold; 95 | } 96 | 97 | bool SteepnessLayer::computeLayer() 98 | { 99 | RCLCPP_INFO_STREAM(node_->get_logger(), "Computing steepness..."); 100 | 101 | lvr2::DenseFaceMap face_normals; 102 | 103 | const auto mesh = map_ptr_->mesh(); 104 | auto mesh_io = map_ptr_->meshIO(); 105 | auto face_normals_opt = mesh_io->getDenseAttributeMap>("face_normals"); 106 | 107 | if (face_normals_opt) 108 | { 109 | face_normals = face_normals_opt.get(); 110 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << face_normals.numValues() << " face normals in map file."); 111 | } 112 | else 113 | { 114 | RCLCPP_INFO_STREAM(node_->get_logger(), "No face normals found in the given map file, computing them..."); 115 | face_normals = lvr2::calcFaceNormals(*mesh); 116 | RCLCPP_INFO_STREAM(node_->get_logger(), "Computed " << face_normals.numValues() << " face normals."); 117 | if (mesh_io->addDenseAttributeMap(face_normals, "face_normals")) 118 | { 119 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved face normals to map file."); 120 | } 121 | else 122 | { 123 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save face normals to map file!"); 124 | return false; 125 | } 126 | } 127 | 128 | lvr2::DenseVertexMap vertex_normals; 129 | auto vertex_normals_opt = mesh_io->getDenseAttributeMap>("vertex_normals"); 130 | 131 | if (vertex_normals_opt) 132 | { 133 | vertex_normals = vertex_normals_opt.get(); 134 | RCLCPP_INFO_STREAM(node_->get_logger(), "Found " << vertex_normals.numValues() << " vertex normals in map file!"); 135 | } 136 | else 137 | { 138 | RCLCPP_INFO_STREAM(node_->get_logger(), "No vertex normals found in the given map file, computing them..."); 139 | vertex_normals = lvr2::calcVertexNormals(*mesh, face_normals); 140 | if (mesh_io->addDenseAttributeMap(vertex_normals, "vertex_normals")) 141 | { 142 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saved vertex normals to map file."); 143 | } 144 | else 145 | { 146 | RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not save vertex normals to map file!"); 147 | return false; 148 | } 149 | } 150 | 151 | steepness_.reserve(mesh->nextVertexIndex()); 152 | 153 | for (size_t i = 0; i < mesh->nextVertexIndex(); i++) 154 | { 155 | auto vH = lvr2::VertexHandle(i); 156 | if (!mesh->containsVertex(vH)) 157 | { 158 | continue; 159 | } 160 | 161 | steepness_.insert(vH, acos(vertex_normals[vH].z)); 162 | } 163 | 164 | return computeLethals(); 165 | } 166 | 167 | lvr2::VertexMap& SteepnessLayer::costs() 168 | { 169 | return steepness_; 170 | } 171 | 172 | rcl_interfaces::msg::SetParametersResult SteepnessLayer::reconfigureCallback(std::vector parameters) 173 | { 174 | rcl_interfaces::msg::SetParametersResult result; 175 | result.successful = true; 176 | 177 | bool has_threshold_changed = false; 178 | for (auto parameter : parameters) { 179 | if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold") { 180 | config_.threshold = parameter.as_double(); 181 | has_threshold_changed = true; 182 | } else if (parameter.get_name() == mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor") { 183 | config_.factor = parameter.as_double(); 184 | } 185 | } 186 | 187 | if (has_threshold_changed) { 188 | RCLCPP_INFO_STREAM(node_->get_logger(), "Recompute lethals and notify change from " << layer_name_ << " due to cfg change."); 189 | computeLethals(); 190 | notifyChange(); 191 | } 192 | 193 | return result; 194 | } 195 | 196 | bool SteepnessLayer::initialize() 197 | { 198 | { // threshold 199 | rcl_interfaces::msg::ParameterDescriptor descriptor; 200 | descriptor.description = "Threshold for the local steepness to be counted as lethal."; 201 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 202 | rcl_interfaces::msg::FloatingPointRange range; 203 | range.from_value = 0.01; 204 | range.to_value = 3.1415; 205 | descriptor.floating_point_range.push_back(range); 206 | config_.threshold = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".threshold", config_.threshold, descriptor); 207 | } 208 | { // factor 209 | rcl_interfaces::msg::ParameterDescriptor descriptor; 210 | descriptor.description = "The local steepness factor to weight this layer."; 211 | descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; 212 | rcl_interfaces::msg::FloatingPointRange range; 213 | range.from_value = 0.0; 214 | range.to_value = 1.0; 215 | descriptor.floating_point_range.push_back(range); 216 | config_.factor = node_->declare_parameter(mesh_map::MeshMap::MESH_MAP_NAMESPACE + "." + layer_name_ + ".factor", config_.factor, descriptor); 217 | } 218 | dyn_params_handler_ = node_->add_on_set_parameters_callback(std::bind( 219 | &SteepnessLayer::reconfigureCallback, this, std::placeholders::_1)); 220 | return true; 221 | } 222 | 223 | } /* namespace mesh_layers */ 224 | -------------------------------------------------------------------------------- /mesh_map/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_map 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | 12 | 1.0.0 (2020-12-16) 13 | ------------------ 14 | * Initial release 15 | -------------------------------------------------------------------------------- /mesh_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mesh_map) 3 | 4 | # DEFAULT RELEASE 5 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 8 | endif() 9 | endif() 10 | 11 | find_package(ament_cmake_ros REQUIRED) 12 | # ROS Deps 13 | set(dependencies 14 | geometry_msgs 15 | mesh_msgs_conversions 16 | pluginlib 17 | rclcpp 18 | tf2 19 | tf2_geometry_msgs 20 | tf2_ros 21 | visualization_msgs 22 | std_srvs 23 | ) 24 | foreach(dep IN LISTS dependencies) 25 | find_package(${dep} REQUIRED) 26 | endforeach() 27 | 28 | find_package(LVR2 REQUIRED) 29 | find_package(assimp REQUIRED) 30 | find_package(MPI) 31 | find_package(PkgConfig REQUIRED) 32 | pkg_check_modules(JSONCPP jsoncpp) 33 | 34 | include_directories( 35 | include 36 | ${LVR2_INCLUDE_DIRS} 37 | ) 38 | 39 | add_library(${PROJECT_NAME} 40 | src/mesh_map.cpp 41 | src/util.cpp 42 | ) 43 | target_include_directories(${PROJECT_NAME} PUBLIC 44 | $ 45 | $ 46 | ${ASSIMP_INCLUDE_DIRS}) 47 | ament_target_dependencies(${PROJECT_NAME} ${dependencies}) 48 | target_link_libraries(${PROJECT_NAME} 49 | ${LVR2_LIBRARIES} 50 | ${ASSIMP_LIBRARIES} 51 | ) 52 | target_compile_definitions(${PROJECT_NAME} 53 | PUBLIC 54 | ${LVR2_DEFINITIONS} 55 | ) 56 | 57 | install(DIRECTORY include/ 58 | DESTINATION include 59 | ) 60 | 61 | install(TARGETS ${PROJECT_NAME} 62 | EXPORT export_${PROJECT_NAME} 63 | LIBRARY DESTINATION lib 64 | ARCHIVE DESTINATION lib 65 | RUNTIME DESTINATION bin 66 | ) 67 | 68 | if(BUILD_TESTING) 69 | # Add test lib with layer plugin(s) 70 | # -> Why a test plugin? Concrete plugin are available in the mesh_layers package. 71 | # However, mesh_map cannot test_depend on it without introducing a circular dependency (on colcon pkg level, the build order would be unclear). 72 | add_library(${PROJECT_NAME}_test_layer_plugin 73 | test/layer_plugin.cpp 74 | ) 75 | ament_target_dependencies(${PROJECT_NAME}_test_layer_plugin pluginlib) 76 | target_link_libraries(${PROJECT_NAME}_test_layer_plugin mesh_map) 77 | pluginlib_export_plugin_description_file(mesh_map test/layer_plugin.xml) 78 | install(TARGETS 79 | ${PROJECT_NAME}_test_layer_plugin 80 | ARCHIVE DESTINATION lib 81 | LIBRARY DESTINATION lib 82 | RUNTIME DESTINATION bin 83 | ) 84 | 85 | # add gmock test suites 86 | find_package(ament_cmake_gmock REQUIRED) 87 | ament_add_gmock(${PROJECT_NAME}_mesh_map_test test/mesh_map_test.cpp) 88 | target_include_directories(${PROJECT_NAME}_mesh_map_test PUBLIC 89 | $ 90 | $ 91 | ) 92 | target_link_libraries(${PROJECT_NAME}_mesh_map_test ${PROJECT_NAME}) 93 | endif() 94 | 95 | ament_export_include_directories(include) 96 | ament_export_libraries(${PROJECT_NAME}) 97 | ament_export_dependencies(${dependencies}) 98 | ament_package() 99 | -------------------------------------------------------------------------------- /mesh_map/include/mesh_map/abstract_layer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #ifndef MESH_MAP__ABSTRACT_LAYER_H 48 | #define MESH_MAP__ABSTRACT_LAYER_H 49 | 50 | namespace mesh_map 51 | { 52 | class MeshMap; 53 | 54 | typedef lvr2::BaseVector Vector; 55 | typedef lvr2::Normal Normal; 56 | 57 | typedef std::function notify_func; 58 | 59 | class AbstractLayer 60 | { 61 | public: 62 | typedef std::shared_ptr Ptr; 63 | 64 | /** 65 | * @brief reads the layer data, e.g. from a file, or a database 66 | * @return true, if the layer data has been read successfully 67 | */ 68 | virtual bool readLayer() = 0; 69 | 70 | /** 71 | * @brief Writes the layer data, e.g. to a file, or a database 72 | * @return true, if the layer data has been written successfully 73 | */ 74 | virtual bool writeLayer() = 0; 75 | 76 | /** 77 | * @brief Defines the default value for all vertex costs which are not set in the layer 78 | * @return The default layer cost value, e.g. nan, 0, or -1, etc. 79 | */ 80 | virtual float defaultValue() = 0; 81 | 82 | /** 83 | * @brief Defines the threshold value to consider vertices as a "lethal" obstacle. 84 | * All vertices with cost values in the layer which a greater than the threshold value 85 | * are marked as "lethal obstacle". 86 | * @return 87 | */ 88 | virtual float threshold() = 0; 89 | 90 | /** 91 | * @brief Function which is called to compute the layer costs. It is called if the 92 | * layer information could not be loaded or if another layer triggered an update 93 | * @return true, if the layer costs have been computed successfully. 94 | */ 95 | virtual bool computeLayer() = 0; 96 | 97 | /** 98 | * @brief Returns a vertex map, which associates a cost to each vertex handle. 99 | * If a vertex handle is not associated with a cost value, i.e. there is no value 100 | * in the map, the mesh_map will use the default value from threshold(). 101 | * @return a vertex map containing floats. 102 | */ 103 | virtual lvr2::VertexMap& costs() = 0; 104 | 105 | /** 106 | * @brief Returns a set of vertex handles which are associated with "lethal" obstacles. 107 | * @return set of vertex handles which are associated with lethal obstalces. 108 | */ 109 | virtual std::set& lethals() = 0; 110 | 111 | /** 112 | * @brief Called by the mesh map if another previously processed layer triggers an update. 113 | * @param added_lethal The "lethal" obstacle vertex handles which are new with respect to the previous call. 114 | * @param removed_lethal Old "lethal" obstacle vertex handles, i.e. vertices which are no "lethal" obstacles anymore. 115 | */ 116 | virtual void updateLethal(std::set& added_lethal, 117 | std::set& removed_lethal) = 0; 118 | 119 | /** 120 | * @brief Optional method if the layer computes vectors. Computes a vector within a triangle using barycentric coordinates. 121 | * @param vertices The three triangle vertices. 122 | * @param barycentric_coords The thee barycentric coordinates. 123 | * @return The vector for the given barycentric coordinates with respect to the corresponding triangle. Default is an vertex with length 0. 124 | */ 125 | virtual lvr2::BaseVector vectorAt(const std::array& vertices, 126 | const std::array& barycentric_coords) 127 | { 128 | return lvr2::BaseVector(); 129 | } 130 | 131 | /** 132 | * @brief Optional vector map. Can be implemented if the layer should also compute vectors. 133 | * If the implmented layer computes a vector field, this method is used to inject 134 | * the vector field into the mesh map. 135 | * @return an optional vector map. 136 | */ 137 | virtual const boost::optional>&> vectorMap() 138 | { 139 | return boost::none; 140 | } 141 | 142 | /** 143 | * @brief Optional method if the layer computes vectors. Computes a vector for a given vertex handle 144 | * @return a vector for the given vertex. Default is an vertex with length 0. 145 | */ 146 | virtual lvr2::BaseVector vectorAt(const lvr2::VertexHandle& vertex) 147 | { 148 | return lvr2::BaseVector(); 149 | } 150 | 151 | /** 152 | * @brief Initializes the layer plugin with the given name. 153 | */ 154 | virtual bool initialize() = 0; 155 | 156 | /** 157 | * @brief Initializes the layer plugin under the mesh_map namespace ans sets some basic attributes. 158 | */ 159 | virtual bool initialize( 160 | const std::string& name, 161 | const notify_func notify_update, 162 | std::shared_ptr map, 163 | const rclcpp::Node::SharedPtr node) 164 | { 165 | layer_name_ = name; 166 | node_ = node; 167 | layer_namespace_ = "mesh_map/" + name; 168 | notify_ = notify_update; 169 | map_ptr_ = map; 170 | return initialize(); 171 | } 172 | 173 | void notifyChange() 174 | { 175 | this->notify_(layer_name_); 176 | } 177 | 178 | protected: 179 | std::string layer_name_; 180 | std::shared_ptr map_ptr_; 181 | 182 | rclcpp::Node::SharedPtr node_; 183 | std::string layer_namespace_; 184 | 185 | private: 186 | notify_func notify_; 187 | }; 188 | 189 | } /* namespace mesh_map */ 190 | 191 | #endif // MESH_MAP__ABSTRACT_LAYER_H 192 | -------------------------------------------------------------------------------- /mesh_map/include/mesh_map/nanoflann_mesh_adaptor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__NANOFLANN_MESH_ADAPTOR_H 39 | #define MESH_MAP__NANOFLANN_MESH_ADAPTOR_H 40 | 41 | #include 42 | #include "nanoflann.hpp" 43 | 44 | namespace mesh_map{ 45 | struct NanoFlannMeshAdaptor 46 | { 47 | const std::shared_ptr>> mesh; 48 | 49 | /// The constructor that sets the data set source 50 | NanoFlannMeshAdaptor(const std::shared_ptr>> mesh) : mesh(mesh) { } 51 | 52 | inline lvr2::Index kdtree_get_point_count() const { return mesh->nextVertexIndex(); } 53 | 54 | inline float kdtree_get_pt(const lvr2::Index idx, const size_t dim) const 55 | { 56 | const lvr2::VertexHandle vH(idx); 57 | if(mesh->containsVertex(vH)) 58 | { 59 | const lvr2::BaseVector& vertex = mesh->getVertexPosition(vH); 60 | if (dim == 0) return vertex.x; 61 | else if (dim == 1) return vertex.y; 62 | else return vertex.z; 63 | } 64 | return std::nanf(""); 65 | } 66 | 67 | template 68 | bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; } 69 | 70 | }; // end of PointCloudAdaptor 71 | } 72 | 73 | #endif /* MESH_MAP__NANOFLANN_MESH_ADAPTOR_H */ 74 | -------------------------------------------------------------------------------- /mesh_map/include/mesh_map/util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020, 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 | * authors: 34 | * Sebastian Pütz 35 | * 36 | */ 37 | 38 | #ifndef MESH_MAP__UTIL_H 39 | #define MESH_MAP__UTIL_H 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | 54 | namespace mesh_map 55 | { 56 | 57 | //! use normals with datatype float 58 | typedef lvr2::Normal Normal; 59 | 60 | //! use vectors with datatype folat 61 | typedef lvr2::BaseVector Vector; 62 | 63 | 64 | lvr2::MeshBufferPtr extractMeshByName(const aiScene* ascene, std::string name); 65 | 66 | /** 67 | * @brief Function to build std_msgs color instances 68 | * @param r red, value between 0 and 1 69 | * @param g green, value between 0 and 1 70 | * @param b blue, value between 0 and 1 71 | * @param a alpha (optional, default is 1.0) 72 | * @return std_msgs/ColorRGBA message 73 | */ 74 | std_msgs::msg::ColorRGBA color(const float& r, const float& g, const float& b, const float& a = 1.0); 75 | 76 | /** 77 | * @brief Function to compute the minimum and maximum of a cost layer 78 | * @param map The cost layer or vertex float map to evaluate 79 | * @param min The computed minimum 80 | * @param max The computed maximum 81 | */ 82 | void getMinMax(const lvr2::VertexMap& map, float& min, float& max); 83 | 84 | /** 85 | * @brief Converts a ROS geometry_msgs/Point message to a lvr2 vector 86 | * @param point The point message to convert 87 | * @return The point converted to a lvr2 vector 88 | */ 89 | Vector toVector(const geometry_msgs::msg::Point& point); 90 | 91 | /** 92 | * Computes a geometry_msgs/Pose message from a position, direction and normal vector 93 | * @param position The position vector 94 | * @param direction The direction vector 95 | * @param normal The normal vector / z-axis 96 | * @return The converted pose message, in the right-hand / ROS coordinate system 97 | */ 98 | geometry_msgs::msg::Pose calculatePoseFromDirection(const Vector& position, const Vector& direction, const Normal& normal); 99 | 100 | /** 101 | * @brief Calculates a geometry_msgs/Pose message from two positions and a normal vector 102 | * @param current The position vector 103 | * @param next The vector in which the x-axis points to 104 | * @param normal The normal vector / z-axis 105 | * @return The converted pose message, in the right-hand / ROS coordinate system 106 | */ 107 | geometry_msgs::msg::Pose calculatePoseFromPosition(const Vector& current, const Vector& next, const Normal& normal); 108 | 109 | /** 110 | * @brief Calculates a geometry_msgs/Pose message from two positions and a normal vector 111 | * @param current The position vector 112 | * @param next The vector in which the x-axis points to 113 | * @param normal The normal vector / z-axis 114 | * @param cost The distance between "current" and "next" 115 | * @return The converted pose message, in the right-hand / ROS coordinate system 116 | */ 117 | geometry_msgs::msg::Pose calculatePoseFromPosition(const Vector& current, const Vector& next, const Normal& normal, 118 | float& cost); 119 | /** 120 | * @brief Projects a vector / point onto a plane, which is defined by the reference vector and the normal vector 121 | * @param vec The point which should be projected onto the plane 122 | * @param ref The plane's reference vector 123 | * @param normal The plane's normal vector 124 | * @return The projected vector 125 | */ 126 | Vector projectVectorOntoPlane(const Vector& vec, const Vector& ref, const Normal& normal); 127 | 128 | /** 129 | * @brief Computes whether a points lies inside or outside a triangle with respect to a maximum distance while using an epsilon 130 | * @param p The query point 131 | * @param v0 First vertex of the triangle 132 | * @param v1 Second vertex of the triangle 133 | * @param v2 Third vertex of the triangle 134 | * @param max_dist The point's maximum distance to the triangle plane 135 | * @param epsilon The epsilon used for the inside / outside check 136 | * @return true if the point lies inside the triangle 137 | */ 138 | bool inTriangle(const Vector& p, const Vector& v0, const Vector& v1, const Vector& v2, const float& max_dist, 139 | const float& epsilon); 140 | 141 | /** 142 | * @brief Computes projected barycentric coordinates and whether the query point lies inside or outside the given triangle 143 | * @param p The query point 144 | * @param vertices The three triangle vertices 145 | * @param barycentric_coords The computed barycentric coordinates 146 | * @param dist The distance from the plane to the point 147 | * @return true if the point lies inside or outside the triangle 148 | */ 149 | bool projectedBarycentricCoords(const Vector& p, const std::array& vertices, 150 | std::array& barycentric_coords, float& dist); 151 | 152 | /** 153 | * @brief Computes the barycentric coordinates u, v,q of a query point p onto the triangle v0, v1, v2 154 | * @param p The query point 155 | * @param v0 First vertex of the triangle 156 | * @param v1 Second vertex of the triangle 157 | * @param v2 Third vertex of the triangle 158 | * @param u First barycentric coordinate 159 | * @param v Second barycentric coordinate 160 | * @param w Third barycentric coordinate 161 | * @return true if the query point lies inside the triangle 162 | */ 163 | bool barycentricCoords(const Vector& p, const Vector& v0, const Vector& v1, const Vector& v2, float& u, float& v, 164 | float& w); 165 | 166 | /** 167 | * @brief Computes a linear combination of vertex properties and the barycentric coordinates 168 | * @tparam T The value to combine with barycentric coordinates, it must support the star/*-operator 169 | * @param vertex_properties The vertex properties of a triangle 170 | * @param barycentric_coords The barycentric coordinates 171 | * @return The linear combined value, e.g. a vector, or cost value 172 | */ 173 | template 174 | T linearCombineBarycentricCoords(const std::array& vertex_properties, 175 | const std::array& barycentric_coords) 176 | { 177 | return vertex_properties[0] * barycentric_coords[0] + vertex_properties[1] * barycentric_coords[1] + 178 | vertex_properties[2] * barycentric_coords[2]; 179 | } 180 | 181 | /** 182 | * @brief Computes a linear combination of vertex properties and the barycentric coordinates 183 | * @tparam T The value to combine with barycentric coordinates, it must support the star/*-operator 184 | * @param vertices The three vertex handles of a triangle 185 | * @param attribute_map An attribute map to access with the given vertex handles 186 | * @param barycentric_coords The barycentric coordinates 187 | * @return The linear combined value, e.g. a vector, or cost value 188 | */ 189 | template 190 | T linearCombineBarycentricCoords(const std::array& vertices, 191 | const lvr2::VertexMap& attribute_map, 192 | const std::array& barycentric_coords) 193 | { 194 | const std::array values = { attribute_map[vertices[0]], attribute_map[vertices[1]], 195 | attribute_map[vertices[2]] }; 196 | 197 | return linearCombineBarycentricCoords(values, barycentric_coords); 198 | } 199 | 200 | /** 201 | * @brief map value to color on color rainbow 202 | * @param value value in range from 0 to 1 203 | * @return color message 204 | */ 205 | std_msgs::msg::ColorRGBA getRainbowColor(const float value); 206 | 207 | /** 208 | * @brief map value to color on color rainbow 209 | * @param value value in range from 0 to 1 210 | * @param[out] r resulting red value 211 | * @param[out] g resulting green value 212 | * @param[out] b resultning blue value 213 | */ 214 | void getRainbowColor(float value, float& r, float& g, float& b); 215 | 216 | /** 217 | * Converts the orientation of a geometry_msgs/PoseStamped message to a direction vector 218 | * @param pose the pose to convert 219 | * @return direction normal vector 220 | */ 221 | mesh_map::Normal poseToDirectionVector( 222 | const geometry_msgs::msg::PoseStamped& pose, 223 | const tf2::Vector3& axis=tf2::Vector3(1,0,0)); 224 | 225 | 226 | /** 227 | * Converts the position of a geometry_msgs/PoseStamped message to a position vector 228 | * @param pose the pose to convert 229 | * @return position vector 230 | */ 231 | mesh_map::Vector poseToPositionVector( 232 | const geometry_msgs::msg::PoseStamped& pose); 233 | 234 | } /* namespace mesh_map */ 235 | 236 | #endif // MESH_MAP__UTIL_H 237 | -------------------------------------------------------------------------------- /mesh_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_map 4 | 2.0.0 5 | The mesh_map package 6 | Matthias Holoch 7 | Sebastian Pütz 8 | BSD 3-Clause 9 | Sebastian Pütz 10 | 11 | geometry_msgs 12 | mesh_msgs_conversions 13 | pluginlib 14 | rclcpp 15 | tf2_geometry_msgs 16 | tf2_ros 17 | tf2 18 | visualization_msgs 19 | std_srvs 20 | assimp 21 | 22 | ament_cmake_gmock 23 | 24 | 25 | ament_cmake 26 | 27 | -------------------------------------------------------------------------------- /mesh_map/test/layer_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace mesh_map { 5 | 6 | class TestLayer : public mesh_map::AbstractLayer { 7 | virtual bool readLayer() override { return true; }; 8 | 9 | virtual bool writeLayer() override { return true; }; 10 | 11 | virtual float threshold() override { return 1.0; }; 12 | 13 | virtual float defaultValue() override { 14 | return std::numeric_limits::infinity(); 15 | } 16 | 17 | virtual bool computeLayer() override{ return true; }; 18 | 19 | virtual lvr2::VertexMap &costs() override { return test_costs_; }; 20 | 21 | virtual std::set &lethals() override { 22 | return lethal_vertices_; 23 | }; 24 | 25 | virtual void 26 | updateLethal(std::set &added_lethal, 27 | std::set &removed_lethal) override{}; 28 | 29 | virtual bool initialize() override { return true; }; 30 | 31 | // set of all current lethal vertices 32 | std::set lethal_vertices_; 33 | lvr2::DenseVertexMap test_costs_; 34 | }; 35 | 36 | PLUGINLIB_EXPORT_CLASS(mesh_map::TestLayer, mesh_map::AbstractLayer) 37 | 38 | } // namespace mesh_map -------------------------------------------------------------------------------- /mesh_map/test/layer_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A mesh layer meant for unit testing. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /mesh_map/test/mesh_map_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace ::testing; 7 | 8 | struct MeshMapTest : public Test 9 | { 10 | protected: 11 | 12 | void SetUp() override { 13 | rclcpp::init(0, nullptr); 14 | } 15 | 16 | // Call this manually at the beginning of each test. 17 | // Allows setting parameter overrides via NodeOptions (mirrors behavior of how parameters are loaded from yaml via launch file for example) 18 | void initNodeAndMeshMap(const rclcpp::NodeOptions nodeOptions = rclcpp::NodeOptions()) { 19 | node_ptr_ = std::make_shared("mesh_map", "test", nodeOptions); 20 | tf_buffer_ptr_ = std::make_shared(node_ptr_->get_clock()); 21 | mesh_map_ptr_ = std::make_shared( 22 | *tf_buffer_ptr_, node_ptr_); 23 | } 24 | 25 | void TearDown() override { 26 | rclcpp::shutdown(); 27 | mesh_map_ptr_.reset(); 28 | tf_buffer_ptr_.reset(); 29 | node_ptr_.reset(); 30 | } 31 | 32 | std::shared_ptr mesh_map_ptr_; 33 | std::shared_ptr tf_buffer_ptr_; 34 | rclcpp::Node::SharedPtr node_ptr_; 35 | }; 36 | 37 | TEST_F(MeshMapTest, loadsSinglePlugin) 38 | { 39 | const std::vector layer_names{"test_layer"}; 40 | initNodeAndMeshMap(rclcpp::NodeOptions() 41 | .append_parameter_override("mesh_map.layers", layer_names) 42 | .append_parameter_override("mesh_map.test_layer.type", "mesh_map/TestLayer") 43 | ); 44 | ASSERT_TRUE(mesh_map_ptr_->loadLayerPlugins()); 45 | EXPECT_THAT(mesh_map_ptr_->layer("test_layer"), NotNull()); 46 | } 47 | 48 | TEST_F(MeshMapTest, loadsMultiplePlugins) 49 | { 50 | const std::vector layer_names{"t3", "t1", "t2"}; 51 | initNodeAndMeshMap(rclcpp::NodeOptions() 52 | .append_parameter_override("mesh_map.layers", layer_names) 53 | .append_parameter_override("mesh_map.t1.type", "mesh_map/TestLayer") 54 | .append_parameter_override("mesh_map.t2.type", "mesh_map/TestLayer") 55 | .append_parameter_override("mesh_map.t3.type", "mesh_map/TestLayer") 56 | ); 57 | ASSERT_TRUE(mesh_map_ptr_->loadLayerPlugins()); 58 | EXPECT_THAT(mesh_map_ptr_->layer("t1"), NotNull()); 59 | EXPECT_THAT(mesh_map_ptr_->layer("t2"), NotNull()); 60 | EXPECT_THAT(mesh_map_ptr_->layer("t3"), NotNull()); 61 | } 62 | 63 | int main(int argc, char** argv) 64 | { 65 | testing::InitGoogleTest(&argc, argv); 66 | return RUN_ALL_TESTS(); 67 | } -------------------------------------------------------------------------------- /mesh_navigation/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_navigation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.0.1 (2021-10-27) 10 | ------------------ 11 | * rename to cvp_mesh_planner 12 | 13 | 1.0.0 (2020-12-16) 14 | ------------------ 15 | * Initial release 16 | 17 | -------------------------------------------------------------------------------- /mesh_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mesh_navigation) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() 5 | -------------------------------------------------------------------------------- /mesh_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_navigation 4 | 2.0.0 5 | The mesh_navigation package provides a layered mesh_map implementation, a Move Base Flex mesh navigation server, as well as mesh navigation plugins for path planning and navigation control. 6 | Matthias Holoch 7 | Sebastian Pütz 8 | BSD-3 9 | Sebastian Pütz 10 | 11 | mbf_mesh_core 12 | mbf_mesh_nav 13 | mesh_controller 14 | mesh_layers 15 | mesh_map 16 | cvp_mesh_planner 17 | dijkstra_mesh_planner 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /source_dependencies.yaml: -------------------------------------------------------------------------------- 1 | # Every repository listed here will get cloned and built during CI runs. 2 | # Use this for repositories that cannot be installed via rosdep. 3 | repositories: 4 | lvr2: 5 | type: git 6 | url: https://github.com/uos/lvr2.git 7 | version: main 8 | mesh_tools: 9 | type: git 10 | url: https://github.com/naturerobots/mesh_tools.git 11 | version: main 12 | move_base_flex: 13 | type: git 14 | url: https://github.com/naturerobots/move_base_flex.git 15 | version: humble 16 | --------------------------------------------------------------------------------