├── .gitattributes ├── .gitmodules ├── LICENSE ├── README.md ├── distributed_teb_demo ├── CMakeLists.txt ├── README.md ├── cfg │ ├── maps │ │ ├── empty │ │ │ ├── map.pgm │ │ │ ├── map.yaml │ │ │ └── stage.world │ │ └── warehouse008 │ │ │ ├── map.pgm │ │ │ ├── map.yaml │ │ │ └── stage.world │ ├── move_base_config │ │ ├── base_local_planner_params.yaml │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ ├── move_base_params.yaml │ │ └── teb_local_planner_params.yaml │ └── rviz │ │ └── warehouse008.rviz ├── launch │ ├── demo_move_base.launch │ ├── map_server.launch │ ├── move_base_recursive.launch │ ├── rviz.launch │ └── stage.launch ├── package.xml └── scripts │ └── distribute_goals_8.py ├── dynamicvoronoi ├── CMakeLists.txt ├── README.md ├── data │ └── testmap.pgm ├── include │ └── dynamicvoronoi │ │ ├── boost_graph.h │ │ ├── boost_voronoi.h │ │ ├── bucketedqueue.h │ │ ├── dynamicvoronoi.h │ │ ├── point.h │ │ ├── voronoi_base.h │ │ ├── voronoi_boost_plugin.h │ │ └── voronoi_grid_plugin.h ├── launch │ └── dynamicvoronoi.launch ├── package.xml ├── plugins.xml └── src │ ├── boost_voronoi.cpp │ ├── bucketedqueue.cpp │ ├── dynamicvoronoi.cpp │ ├── dynamicvoronoi_node.cpp │ ├── voronoi_boost_plugin.cpp │ └── voronoi_grid_plugin.cpp ├── msg_merger ├── CMakeLists.txt ├── include │ └── msg_merger │ │ ├── msg_merger_base.h │ │ └── msg_merger_plugin.h ├── launch │ └── msg_merger.launch ├── package.xml ├── plugins.xml └── src │ ├── msg_merger_node.cpp │ └── msg_merger_plugin.cpp └── robot_scan_matcher ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg └── matcher.cfg ├── include └── robot_scan_matcher │ ├── robot_scan_matcher_base.h │ └── robot_scan_matcher_plugins.h ├── launch └── robot_scan_matcher.launch ├── package.xml ├── robot_scan_matcher_plugins.xml └── src ├── robot_scan_matcher_node.cpp └── robot_scan_matcher_plugins.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "teb_local_planner"] 2 | path = teb_local_planner 3 | url = git@github.com:chungym/teb_local_planner.git 4 | branch = noetic-devel 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, chungym 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | 3. 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # distributed_teb_multi_robot 2 | A distributed TEB planner for multi-robot trajectory planning 3 | 4 | Video on YouTube: 5 | 6 | [![Video](https://img.youtube.com/vi/LnB0i0jpYLM/0.jpg)](https://www.youtube.com/watch?v=LnB0i0jpYLM "Video") 7 | 8 | ### Citation 9 | Y. M. Chung, H. Youssef and M. Roidl, "Distributed Timed Elastic Band (DTEB) Planner: Trajectory Sharing and Collision Prediction for Multi-Robot Systems," 2022 International Conference on Robotics and Automation (ICRA), 2022, pp. 10702-10708, doi: 10.1109/ICRA46639.2022.9811762. 10 | 11 | https://ieeexplore.ieee.org/document/9811762 12 | 13 | ### Demo 14 | 15 | A demo is included for testing and reproducing the results in the vidoe. Parts of the maps and scripts are modified from *tuw_multi_robot* package 16 | 17 | ``` 18 | roslaunch distributed_teb_demo demo_move_base.launch room:=warehouse008 nr_of_robots:=8 19 | rosrun distributed_teb_demo distribute_goals_8.py 20 | ``` 21 | 22 | ### Use in your own projects 23 | 24 | The distributed planners is expected to be run with navigation stack. 25 | 26 | Launch: 27 | 28 | 1. For multi-robot systems, each robot should has its own namespace. 29 | 2. In the move_base node, load the setting for teb_local_planner. 30 | 3. The robot_scan_matcher remove the robot footprint from LaserScan, and output a PointCloud2. (optional if there is no sensor) 31 | 4. The msg_merger receives other robots' trajectories. 32 | 33 | Example 34 | ```xml 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | ``` 64 | 65 | ### License 66 | 67 | The *distributed_teb_multi_robot* package is licensed under the BSD license. 68 | 69 | It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed. 70 | 71 | Some third-party dependencies are included that are licensed under different terms: 72 | - *Eigen*, MPL2 license, http://eigen.tuxfamily.org 73 | - *libg2o* / *g2o* itself is licensed under BSD, but the enabled *csparse_extension* is licensed under LGPL3+, 74 | https://github.com/RainerKuemmerle/g2o. [*CSparse*](http://www.cise.ufl.edu/research/sparse/CSparse/) is included as part of the *SuiteSparse* collection, http://www.suitesparse.com. 75 | - *Boost*, Boost Software License, http://www.boost.org 76 | - *OpenCV*, 4.5.0 and higher: Apache 2 License (OpenCV 4.4.0 and lower: 3-clause BSD license), https://opencv.org/ 77 | 78 | All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details. 79 | -------------------------------------------------------------------------------- /distributed_teb_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(distributed_teb_demo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES tuw_multi_robot_demo 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/tuw_multi_robot_demo.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/tuw_multi_robot_demo_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables and/or libraries for installation 164 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 165 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 166 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 167 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark cpp header files for installation 171 | # install(DIRECTORY include/${PROJECT_NAME}/ 172 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 173 | # FILES_MATCHING PATTERN "*.h" 174 | # PATTERN ".svn" EXCLUDE 175 | # ) 176 | 177 | ## Mark config files for installation 178 | install(DIRECTORY cfg/ 179 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg 180 | ) 181 | 182 | ## Mark launch files for installation 183 | install(DIRECTORY launch/ 184 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 185 | ) 186 | 187 | ############# 188 | ## Testing ## 189 | ############# 190 | 191 | ## Add gtest based cpp test target and link libraries 192 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_tuw_multi_robot_demo.cpp) 193 | # if(TARGET ${PROJECT_NAME}-test) 194 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 195 | # endif() 196 | 197 | ## Add folders to be run by python nosetests 198 | # catkin_add_nosetests(test) 199 | -------------------------------------------------------------------------------- /distributed_teb_demo/README.md: -------------------------------------------------------------------------------- 1 | # distributed\_teb\_demo 2 | Contains launch and config files to run a sample demo. 3 | 4 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/maps/empty/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chungym/distributed_teb_multi_robot/b2f814d064a3dfee98dfb3ed3289e735be5f968a/distributed_teb_demo/cfg/maps/empty/map.pgm -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/maps/empty/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-15.000000, -15.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/maps/empty/stage.world: -------------------------------------------------------------------------------- 1 | define hokuyolaser ranger 2 | ( 3 | sensor( 4 | # laser-specific properties 5 | # factory settings for LMS200 6 | range [ 0.0 5.0 ] 7 | fov 270.0 8 | samples 270 9 | ) 10 | model 11 | ( 12 | # generic model properties 13 | size [ 0.07 0.07 0.05 ] # dimensions from LMS200 data sheet 14 | color "blue" 15 | obstacle_return 0 16 | ) 17 | ) 18 | 19 | 20 | define floorplan model 21 | ( 22 | # sombre, sensible, artistic 23 | color "gray30" 24 | 25 | # most maps will need a bounding box 26 | boundary 1 27 | 28 | gui_nose 0 29 | gui_grid 0 30 | 31 | obstacle_return 1 32 | gui_outline 0 33 | gripper_return 0 34 | fiducial_return 0 35 | ranger_return 1 36 | ) 37 | 38 | 39 | define background model 40 | ( 41 | # sombre, sensible, artistic 42 | color "red" 43 | 44 | # most maps will need a bounding box 45 | boundary 1 46 | 47 | gui_nose 0 48 | gui_grid 0 49 | 50 | obstacle_return 0 51 | gui_outline 0 52 | gripper_return 0 53 | fiducial_return 0 54 | laser_return 1 55 | ) 56 | 57 | 58 | # set the resolution of the underlying raytrace model in meters 59 | resolution 0.02 60 | 61 | interval_sim 100 # simulation timestep in milliseconds 62 | 63 | 64 | window 65 | ( 66 | size [ 1500.000 1500.000 ] 67 | 68 | rotate [ 0.000 0 ] 69 | scale 50 70 | 71 | show_data 1 # 1=on 0=off 72 | ) 73 | 74 | 75 | # load an environment bitmap 76 | 77 | floorplan 78 | ( 79 | name "empty" 80 | bitmap "map.pgm" 81 | size [16.0 16.0 0.5] 82 | pose [ 0 0 0 0 ] 83 | ) 84 | 85 | define pioneer_base position 86 | ( 87 | color "red" # Default color. 88 | drive "diff" # Differential steering model. 89 | gui_nose 0 # Draw a nose on the robot so we can see which way it points 90 | obstacle_return 1 # Can hit things. 91 | ranger_return 0.5 # reflects sonar beams 92 | blob_return 1 # Seen by blobfinders 93 | fiducial_return 1 # Seen as "1" fiducial finders 94 | 95 | localization "gps" 96 | localization_origin [0 0 0 0] # Start odometry at (0, 0, 0). 97 | 98 | # alternative odometric localization with simple error model 99 | # localization "odom" # Change to "gps" to have impossibly perfect, global odometry 100 | # odom_error [ 0.05 0.05 0.1 ] # Odometry error or slip in X, Y and Theta 101 | # (Uniform random distribution) 102 | 103 | # four DOF kinematics limits 104 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 105 | velocity_bounds [-1 1 0 0 0 0 -171.887 171.887 ] 106 | acceleration_bounds [-1.0 1.0 0 0 0 0 -57.296 57.296 ] 107 | ) 108 | 109 | define pioneer2dx_base_no_sonar pioneer_base 110 | ( 111 | # actual size 112 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site 113 | 114 | # the pioneer's center of rotation is offset from its center of area 115 | origin [-0.04 0 0 0] 116 | 117 | # draw a nose on the robot so we can see which way it points 118 | gui_nose 0 119 | 120 | # estimated mass in KG 121 | mass 23.0 122 | ) 123 | 124 | # as above, but with front sonar only 125 | define pioneer2dx_no_sonar pioneer2dx_base_no_sonar 126 | ( 127 | # simplified Body shape: 128 | block( 129 | points 8 130 | point[0] [-0.2 0.12] 131 | point[1] [-0.2 -0.12] 132 | point[2] [-0.12 -0.2555] 133 | point[3] [0.12 -0.2555] 134 | point[4] [0.2 -0.12] 135 | point[5] [0.2 0.12] 136 | point[6] [0.12 0.2555] 137 | point[7] [-0.12 0.2555] 138 | z [0 0.22] 139 | ) 140 | ) 141 | 142 | 143 | pioneer2dx_no_sonar 144 | ( 145 | # can refer to the robot by this name 146 | name "robot_0" 147 | color "red" 148 | pose [ -5 0 0 0 ] 149 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 150 | ) 151 | 152 | pioneer2dx_no_sonar 153 | ( 154 | # can refer to the robot by this name 155 | name "robot_1" 156 | color "yellow" 157 | pose [ 5 0 0 180 ] 158 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 159 | ) 160 | 161 | 162 | pioneer2dx_no_sonar 163 | ( 164 | # can refer to the robot by this name 165 | name "robot_2" 166 | color "blue" 167 | pose [ -2.5 4.33013 0 -60 ] 168 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 169 | ) 170 | 171 | pioneer2dx_no_sonar 172 | ( 173 | # can refer to the robot by this name 174 | name "robot_3" 175 | color "magenta" 176 | pose [ 2.5 -4.33013 0 120 ] 177 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 178 | ) 179 | 180 | pioneer2dx_no_sonar 181 | ( 182 | # can refer to the robot by this name 183 | name "robot_4" 184 | color "green" 185 | pose [ 2.5 4.33013 0 -120 ] 186 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 187 | ) 188 | 189 | pioneer2dx_no_sonar 190 | ( 191 | # can refer to the robot by this name 192 | name "robot_5" 193 | color "white" 194 | pose [ -2.5 -4.33013 0 60 ] 195 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 196 | ) 197 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/maps/warehouse008/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chungym/distributed_teb_multi_robot/b2f814d064a3dfee98dfb3ed3289e735be5f968a/distributed_teb_demo/cfg/maps/warehouse008/map.pgm -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/maps/warehouse008/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.066667 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/maps/warehouse008/stage.world: -------------------------------------------------------------------------------- 1 | define hokuyolaser ranger 2 | ( 3 | sensor( 4 | # laser-specific properties 5 | # factory settings for LMS200 6 | range [ 0.0 5.0 ] 7 | fov 270.0 8 | samples 270 9 | ) 10 | model 11 | ( 12 | # generic model properties 13 | size [ 0.07 0.07 0.05 ] # dimensions from LMS200 data sheet 14 | color "blue" 15 | obstacle_return 0 16 | ) 17 | ) 18 | 19 | 20 | define floorplan model 21 | ( 22 | # sombre, sensible, artistic 23 | color "gray30" 24 | 25 | # most maps will need a bounding box 26 | boundary 1 27 | 28 | gui_nose 0 29 | gui_grid 0 30 | 31 | obstacle_return 1 32 | gui_outline 0 33 | gripper_return 0 34 | fiducial_return 0 35 | ranger_return 1 36 | ) 37 | 38 | 39 | define background model 40 | ( 41 | # sombre, sensible, artistic 42 | color "red" 43 | 44 | # most maps will need a bounding box 45 | boundary 1 46 | 47 | gui_nose 0 48 | gui_grid 0 49 | 50 | obstacle_return 0 51 | gui_outline 0 52 | gripper_return 0 53 | fiducial_return 0 54 | laser_return 1 55 | ) 56 | 57 | 58 | # set the resolution of the underlying raytrace model in meters 59 | resolution 0.02 60 | 61 | interval_sim 100 # simulation timestep in milliseconds 62 | 63 | 64 | window 65 | ( 66 | size [ 635.000 666.000 ] # in pixels 67 | rotate [ 0.000 0 ] 68 | scale 30 69 | show_data 1 # 1=on 0=off 70 | ) 71 | 72 | 73 | # load an environment bitmap 74 | 75 | floorplan 76 | ( 77 | name "warhouse" 78 | bitmap "map.pgm" 79 | size [20.0 20.0 0.5] 80 | pose [ 0 0 0 0 ] 81 | ) 82 | 83 | define pioneer_base position 84 | ( 85 | color "red" # Default color. 86 | drive "diff" # Differential steering model. 87 | gui_nose 0 # Draw a nose on the robot so we can see which way it points 88 | obstacle_return 1 # Can hit things. 89 | ranger_return 0.5 # reflects sonar beams 90 | blob_return 1 # Seen by blobfinders 91 | fiducial_return 1 # Seen as "1" fiducial finders 92 | 93 | localization "gps" 94 | localization_origin [0 0 0 0] # Start odometry at (0, 0, 0). 95 | 96 | # alternative odometric localization with simple error model 97 | # localization "odom" # Change to "gps" to have impossibly perfect, global odometry 98 | # odom_error [ 0.05 0.05 0.1 ] # Odometry error or slip in X, Y and Theta 99 | # (Uniform random distribution) 100 | 101 | # four DOF kinematics limits 102 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 103 | velocity_bounds [-1 1 0 0 0 0 -45.0 45.0 ] 104 | acceleration_bounds [-1.0 1.0 0 0 0 0 -45 45.0 ] 105 | ) 106 | 107 | define pioneer2dx_base_no_sonar pioneer_base 108 | ( 109 | # actual size 110 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site 111 | 112 | # the pioneer's center of rotation is offset from its center of area 113 | origin [-0.04 0 0 0] 114 | 115 | # draw a nose on the robot so we can see which way it points 116 | gui_nose 0 117 | 118 | # estimated mass in KG 119 | mass 23.0 120 | 121 | ) 122 | 123 | # as above, but with front sonar only 124 | define pioneer2dx_no_sonar pioneer2dx_base_no_sonar 125 | ( 126 | # simplified Body shape: 127 | block( 128 | points 8 129 | point[0] [-0.2 0.12] 130 | point[1] [-0.2 -0.12] 131 | point[2] [-0.12 -0.2555] 132 | point[3] [0.12 -0.2555] 133 | point[4] [0.2 -0.12] 134 | point[5] [0.2 0.12] 135 | point[6] [0.12 0.2555] 136 | point[7] [-0.12 0.2555] 137 | z [0 0.22] 138 | ) 139 | block( 140 | points 3 141 | point[0] [ 0.2 0.0] 142 | point[1] [ -0.1 -0.15] 143 | point[2] [ -0.1 0.15] 144 | z [0 0.22] 145 | ) 146 | ) 147 | 148 | 149 | pioneer2dx_no_sonar 150 | ( 151 | # can refer to the robot by this name 152 | name "robot_0" 153 | color "red" 154 | pose [ -6 6 0 -90 ] 155 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 156 | ) 157 | 158 | pioneer2dx_no_sonar 159 | ( 160 | # can refer to the robot by this name 161 | name "robot_1" 162 | color "green" 163 | pose [ -2 6 0 -90 ] 164 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 165 | ) 166 | 167 | pioneer2dx_no_sonar 168 | ( 169 | # can refer to the robot by this name 170 | name "robot_2" 171 | color "blue" 172 | pose [ 2 6 0 -90 ] 173 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 174 | ) 175 | 176 | pioneer2dx_no_sonar 177 | ( 178 | # can refer to the robot by this name 179 | name "robot_3" 180 | color "yellow" 181 | pose [ 6 6 0 -90 ] 182 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 183 | ) 184 | 185 | pioneer2dx_no_sonar 186 | ( 187 | # can refer to the robot by this name 188 | name "robot_4" 189 | color "magenta" 190 | pose [ 6 -6 0 90 ] 191 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 192 | ) 193 | 194 | pioneer2dx_no_sonar 195 | ( 196 | # can refer to the robot by this name 197 | name "robot_5" 198 | color "white" 199 | pose [ 2 -6 0 90 ] 200 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 201 | ) 202 | 203 | pioneer2dx_no_sonar 204 | ( 205 | # can refer to the robot by this name 206 | name "robot_6" 207 | color "black" 208 | pose [ -2 -6 0 90 ] 209 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 210 | ) 211 | 212 | pioneer2dx_no_sonar 213 | ( 214 | # can refer to the robot by this name 215 | name "robot_7" 216 | color "gray" 217 | pose [ -6 -6 0 90 ] 218 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 219 | ) 220 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/move_base_config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | base_local_planner: collvoid_local_planner/CollvoidLocalPlanner # params info in http://wiki.ros.org/base_local_planner 2 | #base_local_planner: dwa_local_planner/DWAPlannerROS 3 | 4 | DWAPlannerROS: 5 | acc_lim_x: 1.0 6 | 7 | #The x acceleration limit of the robot in meters/sec^2 8 | 9 | acc_lim_y: 0.0 10 | 11 | #The y acceleration limit of the robot in meters/sec^2 12 | 13 | acc_lim_th: 1.0 14 | 15 | #The rotational acceleration limit of the robot in radians/sec^2 16 | 17 | max_vel_trans: 1.0 18 | 19 | #The absolute value of the maximum translational velocity for the robot in m/s 20 | 21 | min_vel_trans: 0.0 22 | 23 | #The absolute value of the minimum translational velocity for the robot in m/s 24 | 25 | max_vel_x: 1.0 26 | 27 | #The maximum x velocity for the robot in m/s. 28 | 29 | min_vel_x: 1.0 30 | 31 | #The minimum x velocity for the robot in m/s, negative for backwards motion. 32 | 33 | max_vel_y: 0.0 34 | 35 | #The maximum y velocity for the robot in m/s 36 | 37 | min_vel_y: -0.0 38 | 39 | #The minimum y velocity for the robot in m/s 40 | 41 | max_rot_vel: 3.0 42 | 43 | #The absolute value of the maximum rotational velocity for the robot in rad/s 44 | 45 | min_rot_vel: 0.0 46 | 47 | #The absolute value of the minimum rotational velocity for the robot in rad/s 48 | 49 | yaw_goal_tolerance: 0.1 50 | 51 | #The tolerance in radians for the controller in yaw/rotation when achieving its goal 52 | 53 | sim_time: 5 54 | 55 | #The amount of time to forward-simulate trajectories in seconds 56 | 57 | sim_granularity: 0.025 58 | 59 | #The step size, in meters, to take between points on a given trajectory 60 | 61 | vx_samples: 3.0 62 | 63 | #The number of samples to use when exploring the x velocity space 64 | 65 | vy_samples: 0.0 66 | 67 | #The number of samples to use when exploring the y velocity space 68 | 69 | vth_samples: 20.0 70 | 71 | #The number of samples to use when exploring the theta velocity space 72 | 73 | occdist_scale: 0.01 74 | 75 | #The weighting for how much the controller should attempt to avoid obstacles 76 | 77 | 78 | CollvoidLocalPlanner: 79 | # Robot Configuration Parameters - Kobuki 80 | max_vel_x: 3.0 # 0.55 81 | min_vel_x: -0.2 82 | 83 | max_vel_y: 0.0 # diff drive robot 84 | min_vel_y: 0.0 # diff drive robot 85 | 86 | max_vel_trans: 2.0 # choose slightly less than the base's capability 87 | min_vel_trans: 0.05 # this is the min trans velocity when there is negligible rotational velocity 88 | trans_stopped_vel: 0.05 89 | 90 | max_vel_theta: 3.0 # choose slightly less than the base's capability 91 | min_vel_theta: 0.2 # this is the min angular velocity when there is negligible translational velocity 92 | theta_stopped_vel: 0.2 93 | 94 | acc_lim_x: 1.0 # maximum is theoretically 2.0, but we 95 | acc_lim_theta: 1.0 96 | acc_lim_y: 0.0 # diff drive robot 97 | 98 | # Forward Simulation Parameters 99 | sim_time: 1.2 # 1.7 100 | sim_granularity: 0.04 101 | angular_sim_granularity: 0.2 102 | 103 | # Trajectory Scoring Parameters 104 | path_distance_bias: 5.0 # 32.0 - weighting for how much it should stick to the global path plan 105 | goal_distance_bias: 24.0 # 24.0 - wighting for how much it should attempt to reach its goal 106 | 107 | occdist_scale: 0.05 # 0.01 - weighting for how much the controller should avoid obstacles 108 | forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point 109 | stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. 110 | scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint 111 | max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed. 112 | 113 | # Goal Tolerance Parameters 114 | yaw_goal_tolerance: 0.1 # 0.05 115 | xy_goal_tolerance: 0.1 # 0.10 116 | 117 | holo_robot: false 118 | wheel_base: 0.0 119 | robot_radius: 0.26 120 | 121 | 122 | global_frame_id: map 123 | # base_frame: /robot_0/base_link 124 | max_dist_vo: 1. #for scoring function scale 125 | trunc_time: 10.0 126 | 127 | use_truncation: true 128 | use_polygon_footprint: false # true #footprint or radius 129 | 130 | use_obstacles: false 131 | use_dwa_scoring: false 132 | prune_plan: false 133 | 134 | time_to_holo: 0.4 135 | min_error_holo: 0.02 136 | max_error_holo: 0.10 137 | left_pref: -0.05 138 | 139 | type_vo: 0 #HRVO = 0, RVO = 1, VO = 2 140 | orca: false #orca or VO 141 | clearpath: true #clearpath or sampling 142 | num_samples: 400 #num samples 143 | new_sampling: true 144 | 145 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/move_base_config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | publish_frequency: 5.0 2 | 3 | robot_radius: 0.26 4 | 5 | static_layer: 6 | map_topic: /map 7 | track_unknown_space: False 8 | first_map_only: True 9 | 10 | inflation_layer: 11 | inflation_radius: 0.3 12 | 13 | obstacle_layer: 14 | obstacle_range: 4.9 15 | raytrace_range: 20.5 16 | observation_sources: front_laser 17 | front_laser: 18 | sensor_frame: base_laser_link 19 | data_type: PointCloud2 20 | topic: PointCloud_filtered 21 | marking: true 22 | clearing: true 23 | inf_is_valid: true 24 | observation_persistence: 0.0 25 | 26 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/move_base_config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /robot_map 3 | robot_base_frame: /robot_base_footprint 4 | update_frequency: 2.0 5 | static_map: true 6 | width: 200.0 7 | height: 200.0 8 | # robot radius +safety_delta 0.25 + 0.02 9 | # set to permit drawing a trajectory through the narrowest door 10 | 11 | inflation_layer: 12 | inflation_radius: 0.3 13 | 14 | plugins: 15 | - 16 | name: static_layer 17 | type: "costmap_2d::StaticLayer" 18 | - 19 | name: inflation_layer 20 | type: "costmap_2d::InflationLayer" 21 | 22 | static_map: 23 | lethal_cost_threshold: 70 24 | static_map: true 25 | track_unknown_space: false 26 | unknown_cost_value: 1 27 | trinary_costmap: false 28 | use_maximum: true 29 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/move_base_config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: robot_odom 3 | robot_base_frame: robot_base_footprint 4 | update_frequency: 10.0 5 | always_send_full_costmap: true 6 | static_map: false 7 | rolling_window: true 8 | width: 15.0 9 | height: 15.0 10 | resolution: 0.05 11 | # padding is 0.1 by default, making difficult to pass through narrow places 12 | footprint_padding: 0.0 13 | 14 | inflation_layer: 15 | inflation_radius: 0.15 16 | 17 | plugins: 18 | - 19 | name: static_layer 20 | type: "costmap_2d::StaticLayer" 21 | 22 | - 23 | name: inflation_layer 24 | type: "costmap_2d::InflationLayer" 25 | 26 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/move_base_config/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: global_planner/GlobalPlanner 2 | controller_frequency: 20.0 3 | planner_patience: 5.0 4 | controller_patience: 15.0 5 | conservative_reset_dist: 3.0 6 | recovery_behavior_enabled: true 7 | clearing_rotation_allowed: false 8 | shutdown_costmaps: false 9 | oscillation_timeout: 0.0 10 | oscillation_distance: 0.5 11 | planner_frequency: 0.2 12 | 13 | 14 | #clearing_radius: 0.59 15 | #footprint_padding: 0.03 16 | 17 | #footprint: [[0.133, 0.19], 18 | # [0.203, 0.115], 19 | # [0.203, -0.115], 20 | # [0.133, -0.19], 21 | # [-0.123, -0.19], 22 | # [-0.203, -0.103], 23 | # [-0.203, 0.103], 24 | # [-0.123, 0.19]] 25 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/move_base_config/teb_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | base_local_planner: teb_local_planner/TebLocalPlannerROS 2 | 3 | TebLocalPlannerROS: 4 | odom_topic: /odom 5 | map_frame: robot_odom 6 | 7 | # Trajectory 8 | 9 | teb_autosize: True 10 | dt_ref: 0.3 11 | dt_hysteresis: 0.1 12 | global_plan_overwrite_orientation: True 13 | max_global_plan_lookahead_dist: 5.0 14 | feasibility_check_no_poses: 1 15 | publish_feedback: True 16 | exact_arc_length: true 17 | 18 | # Robot 19 | 20 | max_vel_x: 0.8 #1.0 21 | max_vel_x_backwards: 0.8 22 | max_vel_theta: 1.0 #1.0 23 | acc_lim_x: 1.0 24 | acc_lim_theta: 1.0 25 | min_turning_radius: 0.0 26 | wheelbase: 0.0 27 | cmd_angle_instead_rotvel: False 28 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 29 | type: "circular" 30 | radius: 0.26 # for type "circular" 31 | #line_start: [-0.3, 0.0] # for type "line" 32 | #line_end: [0.3, 0.0] # for type "line" 33 | #front_offset: 0.2 # for type "two_circles" 34 | #front_radius: 0.2 # for type "two_circles" 35 | #rear_offset: 0.2 # for type "two_circles" 36 | #rear_radius: 0.2 # for type "two_circles" 37 | #vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon" 38 | 39 | # GoalTolerance 40 | 41 | xy_goal_tolerance: 0.1 42 | yaw_goal_tolerance: 0.1 43 | free_goal_vel: False 44 | 45 | # Obstacles 46 | 47 | min_obstacle_dist: 0.10 48 | inflation_dist: 0.25 49 | dynamic_obstacle_inflation_dist: 0.25 50 | include_dynamic_obstacles: True 51 | include_obstacle_trajectory: True 52 | prioritised_planning: True 53 | priority_rescheduling: True 54 | include_costmap_obstacles: False 55 | num_trials_before_reschedule: 15 56 | dist_non_prioritised: 1.5 57 | costmap_obstacles_behind_robot_dist: 1.0 58 | obstacle_poses_affected: 30 59 | costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" 60 | #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC" 61 | #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH" 62 | #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull" 63 | #costmap_converter_plugin: "" # deactivate plugin 64 | costmap_converter_spin_thread: True 65 | costmap_converter_rate: 10 66 | #costmap_converter/CostmapToLinesDBSRANSAC: 67 | # cluster_max_distance: 0.4 68 | 69 | 70 | # Optimization 71 | 72 | no_inner_iterations: 5 73 | no_outer_iterations: 4 74 | optimization_activate: True 75 | optimization_verbose: False 76 | penalty_epsilon: 0.05 77 | weight_max_vel_x: 10 78 | weight_max_vel_theta: 5 79 | weight_acc_lim_x: 10 80 | weight_acc_lim_theta: 5 81 | weight_kinematics_nh: 1000 82 | weight_kinematics_forward_drive: 5 83 | weight_kinematics_turning_radius: 1 84 | weight_optimaltime: 1 85 | weight_maxtime: 0.001 86 | weight_obstacle: 100 87 | weight_dynamic_obstacle: 100 # not in use yet 88 | selection_alternative_time_cost: False 89 | 90 | # Homotopy Class Planner 91 | 92 | enable_homotopy_class_planning: True 93 | enable_multithreading: True 94 | selection_cost_hysteresis: 0.85 95 | switching_blocking_period: 0.2 96 | simple_exploration: False 97 | voronoi_exploration: True 98 | max_number_classes: 2 99 | roadmap_graph_no_samples: 15 100 | roadmap_graph_area_width: 5 101 | h_signature_prescaler: 0.5 102 | h_signature_threshold: 0.1 103 | obstacle_keypoint_offset: 0.1 104 | obstacle_heading_threshold: 0.45 105 | visualize_hc_graph: False 106 | max_ratio_detours_duration_best_duration: 5 107 | delete_detours_backwards: True 108 | detours_orientation_tolerance: 3.1416 109 | -------------------------------------------------------------------------------- /distributed_teb_demo/cfg/rviz/warehouse008.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 171 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 367 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | - /Goal Selector1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.699999988079071 56 | Class: rviz/Map 57 | Color Scheme: map 58 | Draw Behind: false 59 | Enabled: true 60 | Name: Map 61 | Topic: /map 62 | Unreliable: false 63 | Use Timestamp: false 64 | Value: true 65 | - Class: VoronoiGraph 66 | Enabled: true 67 | History Length: 1 68 | Name: VoronoiGraph 69 | Path Color: 50; 51; 204 70 | Path Points Scale: 0.10000000149011612 71 | Queue Size: 10 72 | Topic: /segments 73 | Unreliable: false 74 | Value: true 75 | - Class: MultiRobotInfo 76 | Color Pose: 204; 51; 0 77 | Enabled: false 78 | Keep Alive (s): 5 79 | Keep Measurements: 5 80 | Name: MultiRobotInfo 81 | Queue Size: 10 82 | Robots: 83 | {} 84 | Scale Pose: 1 85 | Topic: /robot_info 86 | Unreliable: false 87 | Value: false 88 | - Class: RobotGoalArray 89 | Color Pose: 78; 154; 6 90 | Enabled: true 91 | Name: RobotGoalArray 92 | Queue Size: 10 93 | Scale Pose: 1 94 | Topic: goals 95 | Unreliable: false 96 | Value: true 97 | - Alpha: 1 98 | Buffer Length: 1 99 | Class: rviz/Path 100 | Color: 239; 41; 41 101 | Enabled: true 102 | Head Diameter: 0.30000001192092896 103 | Head Length: 0.20000000298023224 104 | Length: 0.30000001192092896 105 | Line Style: Lines 106 | Line Width: 0.029999999329447746 107 | Name: Path 0 108 | Offset: 109 | X: 0 110 | Y: 0 111 | Z: 0 112 | Pose Color: 255; 85; 255 113 | Pose Style: None 114 | Queue Size: 10 115 | Radius: 0.029999999329447746 116 | Shaft Diameter: 0.10000000149011612 117 | Shaft Length: 0.10000000149011612 118 | Topic: /robot_0/path_unsynced 119 | Unreliable: false 120 | Value: true 121 | - Alpha: 1 122 | Buffer Length: 1 123 | Class: rviz/Path 124 | Color: 25; 255; 0 125 | Enabled: true 126 | Head Diameter: 0.30000001192092896 127 | Head Length: 0.20000000298023224 128 | Length: 0.30000001192092896 129 | Line Style: Lines 130 | Line Width: 0.029999999329447746 131 | Name: Path 1 132 | Offset: 133 | X: 0 134 | Y: 0 135 | Z: 0 136 | Pose Color: 255; 85; 255 137 | Pose Style: None 138 | Queue Size: 10 139 | Radius: 0.029999999329447746 140 | Shaft Diameter: 0.10000000149011612 141 | Shaft Length: 0.10000000149011612 142 | Topic: /robot_1/path_unsynced 143 | Unreliable: false 144 | Value: true 145 | - Alpha: 1 146 | Buffer Length: 1 147 | Class: rviz/Path 148 | Color: 32; 74; 135 149 | Enabled: true 150 | Head Diameter: 0.30000001192092896 151 | Head Length: 0.20000000298023224 152 | Length: 0.30000001192092896 153 | Line Style: Lines 154 | Line Width: 0.029999999329447746 155 | Name: Path 2 156 | Offset: 157 | X: 0 158 | Y: 0 159 | Z: 0 160 | Pose Color: 255; 85; 255 161 | Pose Style: None 162 | Queue Size: 10 163 | Radius: 0.029999999329447746 164 | Shaft Diameter: 0.10000000149011612 165 | Shaft Length: 0.10000000149011612 166 | Topic: /robot_2/path_unsynced 167 | Unreliable: false 168 | Value: true 169 | - Alpha: 1 170 | Buffer Length: 1 171 | Class: rviz/Path 172 | Color: 252; 233; 79 173 | Enabled: true 174 | Head Diameter: 0.30000001192092896 175 | Head Length: 0.20000000298023224 176 | Length: 0.30000001192092896 177 | Line Style: Lines 178 | Line Width: 0.029999999329447746 179 | Name: Path 3 180 | Offset: 181 | X: 0 182 | Y: 0 183 | Z: 0 184 | Pose Color: 255; 85; 255 185 | Pose Style: None 186 | Queue Size: 10 187 | Radius: 0.029999999329447746 188 | Shaft Diameter: 0.10000000149011612 189 | Shaft Length: 0.10000000149011612 190 | Topic: /robot_3/path_unsynced 191 | Unreliable: false 192 | Value: true 193 | - Alpha: 1 194 | Buffer Length: 1 195 | Class: rviz/Path 196 | Color: 255; 0; 171 197 | Enabled: true 198 | Head Diameter: 0.30000001192092896 199 | Head Length: 0.20000000298023224 200 | Length: 0.30000001192092896 201 | Line Style: Lines 202 | Line Width: 0.029999999329447746 203 | Name: Path 4 204 | Offset: 205 | X: 0 206 | Y: 0 207 | Z: 0 208 | Pose Color: 255; 85; 255 209 | Pose Style: None 210 | Queue Size: 10 211 | Radius: 0.029999999329447746 212 | Shaft Diameter: 0.10000000149011612 213 | Shaft Length: 0.10000000149011612 214 | Topic: /robot_4/path_unsynced 215 | Unreliable: false 216 | Value: true 217 | - Alpha: 1 218 | Buffer Length: 1 219 | Class: rviz/Path 220 | Color: 238; 238; 236 221 | Enabled: true 222 | Head Diameter: 0.30000001192092896 223 | Head Length: 0.20000000298023224 224 | Length: 0.30000001192092896 225 | Line Style: Lines 226 | Line Width: 0.029999999329447746 227 | Name: Path 5 228 | Offset: 229 | X: 0 230 | Y: 0 231 | Z: 0 232 | Pose Color: 255; 85; 255 233 | Pose Style: None 234 | Queue Size: 10 235 | Radius: 0.029999999329447746 236 | Shaft Diameter: 0.10000000149011612 237 | Shaft Length: 0.10000000149011612 238 | Topic: /robot_5/path_unsynced 239 | Unreliable: false 240 | Value: true 241 | - Alpha: 1 242 | Buffer Length: 1 243 | Class: rviz/Path 244 | Color: 0; 0; 0 245 | Enabled: true 246 | Head Diameter: 0.30000001192092896 247 | Head Length: 0.20000000298023224 248 | Length: 0.30000001192092896 249 | Line Style: Lines 250 | Line Width: 0.029999999329447746 251 | Name: Path 6 252 | Offset: 253 | X: 0 254 | Y: 0 255 | Z: 0 256 | Pose Color: 255; 85; 255 257 | Pose Style: None 258 | Queue Size: 10 259 | Radius: 0.029999999329447746 260 | Shaft Diameter: 0.10000000149011612 261 | Shaft Length: 0.10000000149011612 262 | Topic: /robot_6/path_unsynced 263 | Unreliable: false 264 | Value: true 265 | - Alpha: 1 266 | Buffer Length: 1 267 | Class: rviz/Path 268 | Color: 136; 138; 133 269 | Enabled: true 270 | Head Diameter: 0.30000001192092896 271 | Head Length: 0.20000000298023224 272 | Length: 0.30000001192092896 273 | Line Style: Lines 274 | Line Width: 0.029999999329447746 275 | Name: Path 7 276 | Offset: 277 | X: 0 278 | Y: 0 279 | Z: 0 280 | Pose Color: 255; 85; 255 281 | Pose Style: None 282 | Queue Size: 10 283 | Radius: 0.029999999329447746 284 | Shaft Diameter: 0.10000000149011612 285 | Shaft Length: 0.10000000149011612 286 | Topic: /robot_7/path_unsynced 287 | Unreliable: false 288 | Value: true 289 | - Alpha: 1 290 | Buffer Length: 1 291 | Class: rviz/Path 292 | Color: 252; 175; 62 293 | Enabled: true 294 | Head Diameter: 0.30000001192092896 295 | Head Length: 0.20000000298023224 296 | Length: 0.30000001192092896 297 | Line Style: Lines 298 | Line Width: 0.029999999329447746 299 | Name: Path 8 300 | Offset: 301 | X: 0 302 | Y: 0 303 | Z: 0 304 | Pose Color: 255; 85; 255 305 | Pose Style: None 306 | Queue Size: 10 307 | Radius: 0.029999999329447746 308 | Shaft Diameter: 0.10000000149011612 309 | Shaft Length: 0.10000000149011612 310 | Topic: /robot_8/path_unsynced 311 | Unreliable: false 312 | Value: true 313 | - Alpha: 1 314 | Arrow Length: 0.30000001192092896 315 | Axes Length: 0.30000001192092896 316 | Axes Radius: 0.009999999776482582 317 | Class: rviz/PoseArray 318 | Color: 255; 25; 0 319 | Enabled: true 320 | Head Length: 0.07000000029802322 321 | Head Radius: 0.029999999329447746 322 | Name: PoseArray 323 | Queue Size: 10 324 | Shaft Length: 0.23000000417232513 325 | Shaft Radius: 0.009999999776482582 326 | Shape: Arrow (Flat) 327 | Topic: /robot_0/move_base/TebLocalPlannerROS/teb_poses 328 | Unreliable: false 329 | Value: true 330 | - Alpha: 1 331 | Arrow Length: 0.30000001192092896 332 | Axes Length: 0.30000001192092896 333 | Axes Radius: 0.009999999776482582 334 | Class: rviz/PoseArray 335 | Color: 255; 25; 0 336 | Enabled: true 337 | Head Length: 0.07000000029802322 338 | Head Radius: 0.029999999329447746 339 | Name: PoseArray 340 | Queue Size: 10 341 | Shaft Length: 0.23000000417232513 342 | Shaft Radius: 0.009999999776482582 343 | Shape: Arrow (Flat) 344 | Topic: /robot_1/move_base/TebLocalPlannerROS/teb_poses 345 | Unreliable: false 346 | Value: true 347 | - Alpha: 1 348 | Arrow Length: 0.30000001192092896 349 | Axes Length: 0.30000001192092896 350 | Axes Radius: 0.009999999776482582 351 | Class: rviz/PoseArray 352 | Color: 255; 25; 0 353 | Enabled: true 354 | Head Length: 0.07000000029802322 355 | Head Radius: 0.029999999329447746 356 | Name: PoseArray 357 | Queue Size: 10 358 | Shaft Length: 0.23000000417232513 359 | Shaft Radius: 0.009999999776482582 360 | Shape: Arrow (Flat) 361 | Topic: /robot_2/move_base/TebLocalPlannerROS/teb_poses 362 | Unreliable: false 363 | Value: true 364 | - Alpha: 1 365 | Arrow Length: 0.30000001192092896 366 | Axes Length: 0.30000001192092896 367 | Axes Radius: 0.009999999776482582 368 | Class: rviz/PoseArray 369 | Color: 255; 25; 0 370 | Enabled: true 371 | Head Length: 0.07000000029802322 372 | Head Radius: 0.029999999329447746 373 | Name: PoseArray 374 | Queue Size: 10 375 | Shaft Length: 0.23000000417232513 376 | Shaft Radius: 0.009999999776482582 377 | Shape: Arrow (Flat) 378 | Topic: /robot_3/move_base/TebLocalPlannerROS/teb_poses 379 | Unreliable: false 380 | Value: true 381 | - Alpha: 1 382 | Arrow Length: 0.30000001192092896 383 | Axes Length: 0.30000001192092896 384 | Axes Radius: 0.009999999776482582 385 | Class: rviz/PoseArray 386 | Color: 255; 25; 0 387 | Enabled: true 388 | Head Length: 0.07000000029802322 389 | Head Radius: 0.029999999329447746 390 | Name: PoseArray 391 | Queue Size: 10 392 | Shaft Length: 0.23000000417232513 393 | Shaft Radius: 0.009999999776482582 394 | Shape: Arrow (Flat) 395 | Topic: /robot_4/move_base/TebLocalPlannerROS/teb_poses 396 | Unreliable: false 397 | Value: true 398 | - Alpha: 1 399 | Arrow Length: 0.30000001192092896 400 | Axes Length: 0.30000001192092896 401 | Axes Radius: 0.009999999776482582 402 | Class: rviz/PoseArray 403 | Color: 255; 25; 0 404 | Enabled: true 405 | Head Length: 0.07000000029802322 406 | Head Radius: 0.029999999329447746 407 | Name: PoseArray 408 | Queue Size: 10 409 | Shaft Length: 0.23000000417232513 410 | Shaft Radius: 0.009999999776482582 411 | Shape: Arrow (Flat) 412 | Topic: /robot_5/move_base/TebLocalPlannerROS/teb_poses 413 | Unreliable: false 414 | Value: true 415 | - Alpha: 1 416 | Arrow Length: 0.30000001192092896 417 | Axes Length: 0.30000001192092896 418 | Axes Radius: 0.009999999776482582 419 | Class: rviz/PoseArray 420 | Color: 255; 25; 0 421 | Enabled: true 422 | Head Length: 0.07000000029802322 423 | Head Radius: 0.029999999329447746 424 | Name: PoseArray 425 | Queue Size: 10 426 | Shaft Length: 0.23000000417232513 427 | Shaft Radius: 0.009999999776482582 428 | Shape: Arrow (Flat) 429 | Topic: /robot_6/move_base/TebLocalPlannerROS/teb_poses 430 | Unreliable: false 431 | Value: true 432 | - Alpha: 1 433 | Arrow Length: 0.30000001192092896 434 | Axes Length: 0.30000001192092896 435 | Axes Radius: 0.009999999776482582 436 | Class: rviz/PoseArray 437 | Color: 255; 25; 0 438 | Enabled: true 439 | Head Length: 0.07000000029802322 440 | Head Radius: 0.029999999329447746 441 | Name: PoseArray 442 | Queue Size: 10 443 | Shaft Length: 0.23000000417232513 444 | Shaft Radius: 0.009999999776482582 445 | Shape: Arrow (Flat) 446 | Topic: /robot_7/move_base/TebLocalPlannerROS/teb_poses 447 | Unreliable: false 448 | Value: true 449 | - Alpha: 1 450 | Class: rviz/Polygon 451 | Color: 25; 255; 0 452 | Enabled: true 453 | Name: Polygon 454 | Queue Size: 10 455 | Topic: /robot_0/convex_hull 456 | Unreliable: false 457 | Value: true 458 | - Alpha: 1 459 | Class: rviz/Polygon 460 | Color: 25; 255; 0 461 | Enabled: true 462 | Name: Polygon 463 | Queue Size: 10 464 | Topic: /robot_1/convex_hull 465 | Unreliable: false 466 | Value: true 467 | - Alpha: 1 468 | Class: rviz/Polygon 469 | Color: 25; 255; 0 470 | Enabled: true 471 | Name: Polygon 472 | Queue Size: 10 473 | Topic: /robot_2/convex_hull 474 | Unreliable: false 475 | Value: true 476 | - Alpha: 1 477 | Class: rviz/Polygon 478 | Color: 25; 255; 0 479 | Enabled: true 480 | Name: Polygon 481 | Queue Size: 10 482 | Topic: /robot_3/convex_hull 483 | Unreliable: false 484 | Value: true 485 | - Alpha: 1 486 | Class: rviz/Polygon 487 | Color: 25; 255; 0 488 | Enabled: true 489 | Name: Polygon 490 | Queue Size: 10 491 | Topic: /robot_4/convex_hull 492 | Unreliable: false 493 | Value: true 494 | - Alpha: 1 495 | Class: rviz/Polygon 496 | Color: 25; 255; 0 497 | Enabled: true 498 | Name: Polygon 499 | Queue Size: 10 500 | Topic: /robot_5/convex_hull 501 | Unreliable: false 502 | Value: true 503 | - Alpha: 1 504 | Class: rviz/Polygon 505 | Color: 25; 255; 0 506 | Enabled: true 507 | Name: Polygon 508 | Queue Size: 10 509 | Topic: /robot_6/convex_hull 510 | Unreliable: false 511 | Value: true 512 | - Alpha: 1 513 | Class: rviz/Polygon 514 | Color: 25; 255; 0 515 | Enabled: true 516 | Name: Polygon 517 | Queue Size: 10 518 | Topic: /robot_7/convex_hull 519 | Unreliable: false 520 | Value: true 521 | - Alpha: 1 522 | Buffer Length: 1 523 | Class: rviz/Path 524 | Color: 25; 255; 0 525 | Enabled: true 526 | Head Diameter: 0.30000001192092896 527 | Head Length: 0.20000000298023224 528 | Length: 0.30000001192092896 529 | Line Style: Lines 530 | Line Width: 0.029999999329447746 531 | Name: Path 532 | Offset: 533 | X: 0 534 | Y: 0 535 | Z: 0 536 | Pose Color: 255; 85; 255 537 | Pose Style: None 538 | Queue Size: 10 539 | Radius: 0.029999999329447746 540 | Shaft Diameter: 0.10000000149011612 541 | Shaft Length: 0.10000000149011612 542 | Topic: /robot_0/move_base/CollvoidLocalPlanner/local_plan 543 | Unreliable: false 544 | Value: true 545 | - Alpha: 1 546 | Buffer Length: 1 547 | Class: rviz/Path 548 | Color: 25; 255; 0 549 | Enabled: true 550 | Head Diameter: 0.30000001192092896 551 | Head Length: 0.20000000298023224 552 | Length: 0.30000001192092896 553 | Line Style: Lines 554 | Line Width: 0.029999999329447746 555 | Name: Path 556 | Offset: 557 | X: 0 558 | Y: 0 559 | Z: 0 560 | Pose Color: 255; 85; 255 561 | Pose Style: None 562 | Queue Size: 10 563 | Radius: 0.029999999329447746 564 | Shaft Diameter: 0.10000000149011612 565 | Shaft Length: 0.10000000149011612 566 | Topic: /robot_1/move_base/CollvoidLocalPlanner/local_plan 567 | Unreliable: false 568 | Value: true 569 | - Alpha: 1 570 | Buffer Length: 1 571 | Class: rviz/Path 572 | Color: 25; 255; 0 573 | Enabled: true 574 | Head Diameter: 0.30000001192092896 575 | Head Length: 0.20000000298023224 576 | Length: 0.30000001192092896 577 | Line Style: Lines 578 | Line Width: 0.029999999329447746 579 | Name: Path 580 | Offset: 581 | X: 0 582 | Y: 0 583 | Z: 0 584 | Pose Color: 255; 85; 255 585 | Pose Style: None 586 | Queue Size: 10 587 | Radius: 0.029999999329447746 588 | Shaft Diameter: 0.10000000149011612 589 | Shaft Length: 0.10000000149011612 590 | Topic: /robot_2/move_base/CollvoidLocalPlanner/local_plan 591 | Unreliable: false 592 | Value: true 593 | - Alpha: 1 594 | Buffer Length: 1 595 | Class: rviz/Path 596 | Color: 25; 255; 0 597 | Enabled: true 598 | Head Diameter: 0.30000001192092896 599 | Head Length: 0.20000000298023224 600 | Length: 0.30000001192092896 601 | Line Style: Lines 602 | Line Width: 0.029999999329447746 603 | Name: Path 604 | Offset: 605 | X: 0 606 | Y: 0 607 | Z: 0 608 | Pose Color: 255; 85; 255 609 | Pose Style: None 610 | Queue Size: 10 611 | Radius: 0.029999999329447746 612 | Shaft Diameter: 0.10000000149011612 613 | Shaft Length: 0.10000000149011612 614 | Topic: /robot_3/move_base/CollvoidLocalPlanner/local_plan 615 | Unreliable: false 616 | Value: true 617 | - Alpha: 1 618 | Buffer Length: 1 619 | Class: rviz/Path 620 | Color: 25; 255; 0 621 | Enabled: true 622 | Head Diameter: 0.30000001192092896 623 | Head Length: 0.20000000298023224 624 | Length: 0.30000001192092896 625 | Line Style: Lines 626 | Line Width: 0.029999999329447746 627 | Name: Path 628 | Offset: 629 | X: 0 630 | Y: 0 631 | Z: 0 632 | Pose Color: 255; 85; 255 633 | Pose Style: None 634 | Queue Size: 10 635 | Radius: 0.029999999329447746 636 | Shaft Diameter: 0.10000000149011612 637 | Shaft Length: 0.10000000149011612 638 | Topic: /robot_4/move_base/CollvoidLocalPlanner/local_plan 639 | Unreliable: false 640 | Value: true 641 | - Alpha: 1 642 | Buffer Length: 1 643 | Class: rviz/Path 644 | Color: 25; 255; 0 645 | Enabled: true 646 | Head Diameter: 0.30000001192092896 647 | Head Length: 0.20000000298023224 648 | Length: 0.30000001192092896 649 | Line Style: Lines 650 | Line Width: 0.029999999329447746 651 | Name: Path 652 | Offset: 653 | X: 0 654 | Y: 0 655 | Z: 0 656 | Pose Color: 255; 85; 255 657 | Pose Style: None 658 | Queue Size: 10 659 | Radius: 0.029999999329447746 660 | Shaft Diameter: 0.10000000149011612 661 | Shaft Length: 0.10000000149011612 662 | Topic: /robot_5/move_base/CollvoidLocalPlanner/local_plan 663 | Unreliable: false 664 | Value: true 665 | - Alpha: 1 666 | Buffer Length: 1 667 | Class: rviz/Path 668 | Color: 25; 255; 0 669 | Enabled: true 670 | Head Diameter: 0.30000001192092896 671 | Head Length: 0.20000000298023224 672 | Length: 0.30000001192092896 673 | Line Style: Lines 674 | Line Width: 0.029999999329447746 675 | Name: Path 676 | Offset: 677 | X: 0 678 | Y: 0 679 | Z: 0 680 | Pose Color: 255; 85; 255 681 | Pose Style: None 682 | Queue Size: 10 683 | Radius: 0.029999999329447746 684 | Shaft Diameter: 0.10000000149011612 685 | Shaft Length: 0.10000000149011612 686 | Topic: /robot_6/move_base/CollvoidLocalPlanner/local_plan 687 | Unreliable: false 688 | Value: true 689 | - Alpha: 1 690 | Buffer Length: 1 691 | Class: rviz/Path 692 | Color: 25; 255; 0 693 | Enabled: true 694 | Head Diameter: 0.30000001192092896 695 | Head Length: 0.20000000298023224 696 | Length: 0.30000001192092896 697 | Line Style: Lines 698 | Line Width: 0.029999999329447746 699 | Name: Path 700 | Offset: 701 | X: 0 702 | Y: 0 703 | Z: 0 704 | Pose Color: 255; 85; 255 705 | Pose Style: None 706 | Queue Size: 10 707 | Radius: 0.029999999329447746 708 | Shaft Diameter: 0.10000000149011612 709 | Shaft Length: 0.10000000149011612 710 | Topic: /robot_7/move_base/CollvoidLocalPlanner/local_plan 711 | Unreliable: false 712 | Value: true 713 | Enabled: true 714 | Global Options: 715 | Background Color: 48; 48; 48 716 | Default Light: true 717 | Fixed Frame: map 718 | Frame Rate: 30 719 | Name: root 720 | Tools: 721 | - Class: rviz/Interact 722 | Hide Inactive Objects: true 723 | - Class: rviz/MoveCamera 724 | - Class: rviz/Select 725 | - Class: rviz/FocusCamera 726 | - Class: rviz/Measure 727 | - Class: rviz/SetInitialPose 728 | Theta std deviation: 0.2617993950843811 729 | Topic: /initialpose 730 | X std deviation: 0.5 731 | Y std deviation: 0.5 732 | - Class: rviz/SetGoal 733 | Topic: /move_base_simple/goal 734 | - Class: rviz/PublishPoint 735 | Single click: true 736 | Topic: /clicked_point 737 | - Class: Goal Selector 738 | No. robtos: 3 739 | Robot Goals: 740 | {} 741 | Robot Names: 742 | Robot 0: robot_0 743 | Robot 1: robot_1 744 | Robot 2: robot_2 745 | Value: true 746 | Views: 747 | Current: 748 | Angle: 7.431954145431519e-07 749 | Class: rviz/TopDownOrtho 750 | Enable Stereo Rendering: 751 | Stereo Eye Separation: 0.05999999865889549 752 | Stereo Focal Distance: 1 753 | Swap Stereo Eyes: false 754 | Value: false 755 | Invert Z Axis: false 756 | Name: Current View 757 | Near Clip Distance: 0.009999999776482582 758 | Scale: 31.34200096130371 759 | Target Frame: 760 | X: -0.11219675838947296 761 | Y: -0.30556151270866394 762 | Saved: ~ 763 | Window Geometry: 764 | Displays: 765 | collapsed: false 766 | Height: 752 767 | Hide Left Dock: false 768 | Hide Right Dock: true 769 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000257fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000257000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000318fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000318000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f300000039fc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003970000025700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 770 | Selection: 771 | collapsed: false 772 | Time: 773 | collapsed: false 774 | Tool Properties: 775 | collapsed: false 776 | Views: 777 | collapsed: true 778 | Width: 1267 779 | X: 653 780 | Y: 27 781 | -------------------------------------------------------------------------------- /distributed_teb_demo/launch/demo_move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /distributed_teb_demo/launch/map_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /distributed_teb_demo/launch/move_base_recursive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /distributed_teb_demo/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /distributed_teb_demo/launch/stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /distributed_teb_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | distributed_teb_demo 4 | 0.0.0 5 | Contains launch and config files to run a sample demo. 6 | 7 | 8 | 9 | 10 | example 11 | 12 | 13 | 14 | 15 | BSD 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Yiu Ming Chung 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | map_server 53 | stage_ros 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /distributed_teb_demo/scripts/distribute_goals_8.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | from std_msgs.msg import String 4 | from geometry_msgs.msg import Pose, PoseStamped 5 | 6 | def sendGoals(msg): 7 | 8 | pair_list = [] 9 | 10 | for robot in msg: 11 | pub = rospy.Publisher(robot[0] + '/move_base_simple/goal', PoseStamped, queue_size=1) 12 | new_msg = PoseStamped() 13 | new_msg.header.frame_id = "map" 14 | new_msg.header.stamp = rospy.Time.now() 15 | new_msg.pose = robot[1] 16 | 17 | pair_list.append([pub,new_msg]) 18 | 19 | rospy.sleep(2.0) 20 | 21 | for pair in pair_list: 22 | pair[0].publish(pair[1]) 23 | rospy.sleep(0.001) 24 | 25 | 26 | #for robot in msg.robots: 27 | # pub.unregister() 28 | 29 | def talker(): 30 | 31 | rospy.init_node('goal_distributor', anonymous=True) 32 | 33 | #sub = rospy.Subscriber('goals', RobotGoalsArray, callback) 34 | 35 | pose = Pose() 36 | pose.position.x = -6 37 | pose.position.y = -7 38 | pose.orientation.z = -0.7071068 39 | pose.orientation.w = 0.7071068 40 | robot1 = ["robot_0", pose] 41 | 42 | pose2 = Pose() 43 | pose2.position.x = -2 44 | pose2.position.y = -7 45 | pose2.orientation.z = -0.7071068 46 | pose2.orientation.w = 0.7071068 47 | robot2 = ["robot_1", pose2] 48 | 49 | pose3 = Pose() 50 | pose3.position.x = 2 51 | pose3.position.y = -7 52 | pose3.orientation.z = -0.7071068 53 | pose3.orientation.w = 0.7071068 54 | robot3 = ["robot_2", pose3] 55 | 56 | pose4 = Pose() 57 | pose4.position.x = 6 58 | pose4.position.y = -7 59 | pose4.orientation.z = -0.7071068 60 | pose4.orientation.w = 0.7071068 61 | robot4 = ["robot_3", pose4] 62 | 63 | pose5 = Pose() 64 | pose5.position.x = 6 65 | pose5.position.y = 7 66 | pose5.orientation.z = 0.7071068 67 | pose5.orientation.w = 0.7071068 68 | robot5 = ["robot_4", pose5] 69 | 70 | pose6 = Pose() 71 | pose6.position.x = 2 72 | pose6.position.y = 7 73 | pose6.orientation.z = 0.7071068 74 | pose6.orientation.w = 0.7071068 75 | robot6 = ["robot_5", pose6] 76 | 77 | pose7 = Pose() 78 | pose7.position.x = -2 79 | pose7.position.y = 7 80 | pose7.orientation.z = 0.7071068 81 | pose7.orientation.w = 0.7071068 82 | robot7 = ["robot_6", pose7] 83 | 84 | pose8 = Pose() 85 | pose8.position.x = -6 86 | pose8.position.y = 7 87 | pose8.orientation.z = 0.7071068 88 | pose8.orientation.w = 0.7071068 89 | robot8 = ["robot_7", pose8] 90 | 91 | 92 | msg = [robot1, robot2, robot3, robot4, robot5, robot6, robot7, robot8] 93 | 94 | sendGoals(msg) 95 | 96 | if __name__ == '__main__': 97 | 98 | talker() 99 | 100 | -------------------------------------------------------------------------------- /dynamicvoronoi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(dynamicvoronoi) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | pluginlib 11 | roscpp 12 | costmap_2d 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | find_package(Boost REQUIRED) 17 | find_package(OpenCV REQUIRED) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 36 | ## pulled in transitively but can be declared for certainty nonetheless: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################### 77 | ## catkin specific configuration ## 78 | ################################### 79 | ## The catkin_package macro generates cmake config files for your package 80 | ## Declare things to be passed to dependent projects 81 | ## INCLUDE_DIRS: uncomment this if you package contains header files 82 | ## LIBRARIES: libraries you create in this project that dependent projects also need 83 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 84 | ## DEPENDS: system dependencies of this project that dependent projects also need 85 | catkin_package( 86 | INCLUDE_DIRS include 87 | LIBRARIES dynamicvoronoi 88 | CATKIN_DEPENDS roscpp pluginlib costmap_2d 89 | DEPENDS Boost OpenCV 90 | ) 91 | 92 | ########### 93 | ## Build ## 94 | ########### 95 | 96 | ## Specify additional locations of header files 97 | ## Your package locations should be listed before other locations 98 | include_directories(include) 99 | include_directories( 100 | ${catkin_INCLUDE_DIRS} 101 | ) 102 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 103 | 104 | ## Declare a cpp library 105 | add_library(dynamicvoronoi 106 | src/bucketedqueue.cpp src/dynamicvoronoi.cpp src/voronoi_grid_plugin.cpp src/voronoi_boost_plugin.cpp src/boost_voronoi.cpp 107 | ) 108 | 109 | ## Declare a cpp executable 110 | add_executable(dynamicvoronoi_node src/dynamicvoronoi_node.cpp) 111 | 112 | ## Add cmake target dependencies of the executable/library 113 | ## as an example, message headers may need to be generated before nodes 114 | add_dependencies(dynamicvoronoi_node dynamicvoronoi) 115 | 116 | 117 | ## Specify libraries to link a library or executable target against 118 | target_link_libraries(dynamicvoronoi_node 119 | ${catkin_LIBRARIES} 120 | ${OpenCV_LIBS} 121 | ) 122 | 123 | target_link_libraries(dynamicvoronoi 124 | ${catkin_LIBRARIES} 125 | ${OpenCV_LIBS} 126 | ) 127 | 128 | message(STATUS "OpenCV_LIBS = ${OpenCV_LIBS}") 129 | 130 | ############# 131 | ## Install ## 132 | ############# 133 | 134 | # all install targets should use catkin DESTINATION variables 135 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 136 | 137 | ## Mark executable scripts (Python etc.) for installation 138 | ## in contrast to setup.py, you can choose the destination 139 | # install(PROGRAMS 140 | # scripts/my_python_script 141 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 142 | # ) 143 | 144 | ## Mark executables and/or libraries for installation 145 | install(TARGETS dynamicvoronoi dynamicvoronoi_node 146 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 147 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 148 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 149 | ) 150 | 151 | ## Mark cpp header files for installation 152 | install(DIRECTORY include/${PROJECT_NAME}/ 153 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 154 | FILES_MATCHING PATTERN "*.h" 155 | PATTERN ".svn" EXCLUDE 156 | ) 157 | 158 | ## Mark other files for installation (e.g. launch and bag files, etc.) 159 | install(FILES plugins.xml 160 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 161 | 162 | ############# 163 | ## Testing ## 164 | ############# 165 | 166 | ## Add gtest based cpp test target and link libraries 167 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_dynamicvoronoi.cpp) 168 | # if(TARGET ${PROJECT_NAME}-test) 169 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 170 | # endif() 171 | 172 | ## Add folders to be run by python nosetests 173 | # catkin_add_nosetests(test) 174 | -------------------------------------------------------------------------------- /dynamicvoronoi/README.md: -------------------------------------------------------------------------------- 1 | # dynamic_voronoi 2 | -------------------------------------------------------------------------------- /dynamicvoronoi/data/testmap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chungym/distributed_teb_multi_robot/b2f814d064a3dfee98dfb3ed3289e735be5f968a/dynamicvoronoi/data/testmap.pgm -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/boost_graph.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef _BOOST_GRAPH_H_ 34 | #define _BOOST_GRAPH_H_ 35 | 36 | 37 | #include 38 | 39 | #ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS 40 | #include 41 | #else 42 | // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48: 43 | // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated, 44 | // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now. 45 | #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS 46 | #include 47 | #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS 48 | #endif 49 | 50 | #ifndef GRAPH_SEARCH_INTERFACE_H 51 | 52 | namespace teb_local_planner 53 | { 54 | struct HcGraphVertex 55 | { 56 | public: 57 | Eigen::Vector2d pos; // position of vertices in the map 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 59 | }; 60 | 61 | //! Abbrev. for the homotopy class search-graph type @see HcGraphVertex 62 | typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph; 63 | //! Abbrev. for vertex type descriptors in the homotopy class search-graph 64 | typedef boost::graph_traits::vertex_descriptor HcGraphVertexType; 65 | //! Abbrev. for edge type descriptors in the homotopy class search-graph 66 | typedef boost::graph_traits::edge_descriptor HcGraphEdgeType; 67 | //! Abbrev. for the vertices iterator of the homotopy class search-graph 68 | typedef boost::graph_traits::vertex_iterator HcGraphVertexIterator; 69 | //! Abbrev. for the edges iterator of the homotopy class search-graph 70 | typedef boost::graph_traits::edge_iterator HcGraphEdgeIterator; 71 | //! Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one 72 | typedef boost::graph_traits::adjacency_iterator HcGraphAdjecencyIterator; 73 | 74 | } 75 | 76 | #endif 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/boost_voronoi.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef _BOOST_VORONOI_H_ 34 | #define _BOOST_VORONOI_H_ 35 | 36 | 37 | //system 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | //ROS 47 | #include 48 | #include 49 | 50 | //package 51 | #include 52 | 53 | 54 | 55 | namespace dynamicvoronoi{ 56 | 57 | struct Point { 58 | int a; 59 | int b; 60 | Point(int x, int y) : a(x), b(y) {} 61 | }; 62 | 63 | struct Segment { 64 | Point p0; 65 | Point p1; 66 | Segment(int x1, int y1, int x2, int y2) : p0(x1, y1), p1(x2, y2) {} 67 | }; 68 | 69 | 70 | 71 | struct MyHash { 72 | size_t operator()(const Eigen::Vector2d& vertex) const { 73 | 74 | 75 | std::size_t seed = 0; 76 | boost::hash_combine(seed, boost::hash_value(vertex.x())); 77 | boost::hash_combine(seed, boost::hash_value(vertex.y())); 78 | return seed; 79 | 80 | 81 | // what if duplicate vertices? 82 | //return std::hash*>()(&vertex); 83 | 84 | } 85 | }; 86 | 87 | 88 | 89 | class BoostVoronoi 90 | { 91 | public: 92 | 93 | BoostVoronoi(); 94 | ~BoostVoronoi(); 95 | 96 | boost::mutex* getMutex(); 97 | boost::mutex* getGridMutex(); 98 | 99 | void updateGraph(); 100 | 101 | void setCostmap2D(costmap_2d::Costmap2D *costmap, std::string global_frame_id); 102 | void disableCostmap2D(); 103 | 104 | void setOccupancyGrid(nav_msgs::OccupancyGrid::Ptr grid); 105 | 106 | void updateGridFromCostmap(); 107 | 108 | void addCircularObstacle(double x, double y, double r); 109 | void addLineObstacle(const Eigen::Vector2d& pt1, const Eigen::Vector2d& pt2, double t); 110 | void addPillObstacle(const Eigen::Vector2d& pt1, const Eigen::Vector2d& pt2, double r); 111 | void addPolygonObstacle(const std::vector>& points); 112 | 113 | boost::optional> addStartAndGoal(const Eigen::Vector2d& start, const Eigen::Vector2d& end); 114 | 115 | boost::shared_ptr getGraph(); 116 | 117 | const nav_msgs::OccupancyGrid& getVisualisation(); 118 | 119 | private: 120 | 121 | bool voronoi_initialized_; 122 | 123 | char* cost_translation_table_; 124 | 125 | teb_local_planner::HcGraph graph_; 126 | boost::mutex* graph_mutex_; 127 | 128 | costmap_2d::Costmap2D* costmap_; //!< costmap 129 | std::string global_frame_; 130 | bool directCostmap_; 131 | 132 | nav_msgs::OccupancyGrid::Ptr current_grid_; 133 | 134 | cv::Mat image_; 135 | 136 | nav_msgs::OccupancyGrid output; 137 | boost::mutex* og_mutex_; 138 | 139 | int sizeX_; 140 | int sizeY_; 141 | double resolution_; 142 | Eigen::Vector2d origin_; 143 | 144 | boost::polygon::voronoi_diagram vd; 145 | 146 | }; 147 | 148 | 149 | } // namespace dynamicvoronoi 150 | 151 | 152 | namespace boost { 153 | namespace polygon { 154 | 155 | template <> 156 | struct geometry_concept { 157 | typedef point_concept type; 158 | }; 159 | 160 | template <> 161 | struct point_traits { 162 | typedef int coordinate_type; 163 | 164 | static inline coordinate_type get( 165 | const dynamicvoronoi::Point& point, orientation_2d orient) { 166 | return (orient == HORIZONTAL) ? point.a : point.b; 167 | } 168 | }; 169 | 170 | template <> 171 | struct geometry_concept { 172 | typedef segment_concept type; 173 | }; 174 | 175 | template <> 176 | struct segment_traits { 177 | typedef int coordinate_type; 178 | typedef dynamicvoronoi::Point point_type; 179 | 180 | static inline point_type get(const dynamicvoronoi::Segment& segment, direction_1d dir) { 181 | return dir.to_int() ? segment.p1 : segment.p0; 182 | } 183 | }; 184 | 185 | } // polygon 186 | } // boost 187 | 188 | 189 | 190 | #endif 191 | -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/bucketedqueue.h: -------------------------------------------------------------------------------- 1 | // for comparison only 2 | // source: http://www2.informatik.uni-freiburg.de/~lau/dynamicvoronoi/ 3 | // BSD licence 4 | 5 | #ifndef _PRIORITYQUEUE2_H_ 6 | #define _PRIORITYQUEUE2_H_ 7 | 8 | #define MAXDIST 1000 9 | #define RESERVE 64 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "point.h" 16 | 17 | //! Priority queue for integer coordinates with squared distances as priority. 18 | /** A priority queue that uses buckets to group elements with the same priority. 19 | * The individual buckets are unsorted, which increases efficiency if these groups are large. 20 | * The elements are assumed to be integer coordinates, and the priorities are assumed 21 | * to be squared euclidean distances (integers). 22 | */ 23 | class BucketPrioQueue { 24 | 25 | public: 26 | //! Standard constructor 27 | /** Standard constructor. When called for the first time it creates a look up table 28 | * that maps square distanes to bucket numbers, which might take some time... 29 | */ 30 | BucketPrioQueue(); 31 | //! Checks whether the Queue is empty 32 | bool empty(); 33 | //! push an element 34 | void push(int prio, INTPOINT t); 35 | //! return and pop the element with the lowest squared distance */ 36 | INTPOINT pop(); 37 | 38 | private: 39 | 40 | static void initSqrIndices(); 41 | static std::vector sqrIndices; 42 | static int numBuckets; 43 | int count; 44 | int nextBucket; 45 | 46 | std::vector > buckets; 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/dynamicvoronoi.h: -------------------------------------------------------------------------------- 1 | // for comparison only 2 | // source: http://www2.informatik.uni-freiburg.de/~lau/dynamicvoronoi/ 3 | // BSD licence 4 | 5 | #ifndef _DYNAMICVORONOI_H_ 6 | #define _DYNAMICVORONOI_H_ 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "bucketedqueue.h" 15 | 16 | 17 | #include 18 | 19 | #include 20 | 21 | //! A DynamicVoronoi object computes and updates a distance map and Voronoi diagram. 22 | class DynamicVoronoi { 23 | 24 | public: 25 | DynamicVoronoi(); 26 | ~DynamicVoronoi(); 27 | 28 | void setMapInfo(double resolution, double x, double y); 29 | 30 | //! Initialization with an empty map 31 | void initializeEmpty(int _sizeX, int _sizeY, bool initGridMap=true); 32 | //! Initialization with a given binary map (false==free, true==occupied) 33 | void initializeMap(int _sizeX, int _sizeY, bool** _gridMap); 34 | 35 | //! add an obstacle at the specified cell coordinate 36 | void occupyCell(int x, int y); 37 | //! remove an obstacle at the specified cell coordinate 38 | void clearCell(int x, int y); 39 | //! remove old dynamic obstacles and add the new ones 40 | void exchangeObstacles(std::vector newObstacles); 41 | 42 | //! update distance map and Voronoi diagram to reflect the changes 43 | void update(bool updateRealDist=true); 44 | //! prune the Voronoi diagram 45 | void prune(); 46 | //! connect bi-directional directed graph graph 47 | void createGraph(); 48 | //! connect bi-directional directed graph graph 49 | void createTEBGraph(); 50 | //! search between start and goal 51 | void search(int start_x, int start_y, int goal_x, int goal_y); 52 | //! add start and goal without search 53 | teb_local_planner::HcGraph getGraph(int start_x, int start_y, int goal_x, int goal_y); 54 | //! get the results of search 55 | std::vector >> getPaths(); 56 | std::vector >> getPaths_world(); 57 | 58 | //! returns the obstacle distance at the specified location 59 | float getDistance( int x, int y ); 60 | //! returns whether the specified cell is part of the (pruned) Voronoi graph 61 | bool isVoronoi( int x, int y ); 62 | //! checks whether the specficied location is occupied 63 | bool isOccupied(int x, int y); 64 | //! write the current distance map and voronoi diagram as ppm file 65 | void visualize(const char* filename="result.ppm"); 66 | 67 | //! returns the horizontal size of the workspace/map 68 | unsigned int getSizeX() {return sizeX;} 69 | //! returns the vertical size of the workspace/map 70 | unsigned int getSizeY() {return sizeY;} 71 | 72 | void move(int minX_s_, int minY_s_, int minX_t_, int minY_t_, int sizeX_, int sizeY); 73 | 74 | private: 75 | struct dataCell { 76 | float dist; 77 | char voronoi; 78 | char queueing; 79 | int obstX; 80 | int obstY; 81 | bool needsRaise; 82 | int sqdist; 83 | }; 84 | 85 | typedef enum {voronoiKeep=-4, freeQueued = -3, voronoiRetry=-2, voronoiPrune=-1, free=0, occupied=1} State; 86 | typedef enum {fwNotQueued=1, fwQueued=2, fwProcessed=3, bwQueued=4, bwProcessed=1} QueueingState; 87 | typedef enum {invalidObstData = SHRT_MAX/2} ObstDataState; 88 | typedef enum {pruned, keep, retry} markerMatchResult; 89 | 90 | 91 | // methods 92 | void setObstacle(int x, int y); 93 | void removeObstacle(int x, int y); 94 | inline void checkVoro(int x, int y, int nx, int ny, dataCell& c, dataCell& nc); 95 | void recheckVoro(); 96 | void commitAndColorize(bool updateRealDist=true); 97 | inline void reviveVoroNeighbors(int &x, int &y); 98 | 99 | inline bool isOccupied(int &x, int &y, dataCell &c); 100 | inline markerMatchResult markerMatch(int x, int y); 101 | 102 | //! depth first visiting 103 | void DepthFirst(teb_local_planner::HcGraph& g, std::vector& visited, bool*& visited_array, const teb_local_planner::HcGraphVertexType& goal, int depth); 104 | //! start from query point, spirally visit cells to find the first cell that is voronoi 105 | boost::optional> spiralIsVoronoi( int q_x, int q_y); 106 | 107 | // queues 108 | 109 | BucketPrioQueue open; 110 | std::queue pruneQueue; 111 | 112 | std::vector removeList; 113 | std::vector addList; 114 | std::vector lastObstacles; 115 | 116 | // maps 117 | int sizeY; 118 | int sizeX; 119 | dataCell** data; 120 | bool** gridMap; 121 | 122 | teb_local_planner::HcGraph graph; 123 | teb_local_planner::HcGraphVertexType** vertices; 124 | std::vector> xy_list; 125 | 126 | std::vector >> paths; 127 | std::vector >> paths_world; 128 | 129 | // parameters 130 | int padding; 131 | double doubleThreshold; 132 | 133 | double sqrt2; 134 | 135 | double resolution_; 136 | double origin_x; 137 | double origin_y; 138 | 139 | // dataCell** getData(){ return data; } 140 | }; 141 | 142 | 143 | #endif 144 | 145 | -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/point.h: -------------------------------------------------------------------------------- 1 | // for comparison only 2 | // source: http://www2.informatik.uni-freiburg.de/~lau/dynamicvoronoi/ 3 | // BSD licence 4 | 5 | #ifndef _VOROPOINT_H_ 6 | #define _VOROPOINT_H_ 7 | 8 | #define INTPOINT IntPoint 9 | 10 | /*! A light-weight integer point with fields x,y */ 11 | class IntPoint { 12 | public: 13 | IntPoint() : x(0), y(0) {} 14 | IntPoint(int _x, int _y) : x(_x), y(_y) {} 15 | int x,y; 16 | }; 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/voronoi_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef VORONOI_BASE_H_ 34 | #define VORONOI_BASE_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | // message types 42 | #include 43 | #include 44 | #include 45 | 46 | namespace dynamicvoronoi 47 | { 48 | class BaseVoronoi 49 | { 50 | public: 51 | 52 | virtual ~BaseVoronoi(){stopWorker();} 53 | 54 | virtual void initialize(ros::NodeHandle nh) = 0; 55 | 56 | virtual void setCostmapTopic(std::string costmap_topic, int sizeX, int sizeY, double resolution){}; 57 | virtual void setCostmap2D(costmap_2d::Costmap2D *costmap, std::string global_frame_id){}; 58 | 59 | /** 60 | * @brief This method performs the actual work 61 | */ 62 | virtual void compute() = 0; 63 | 64 | 65 | /** 66 | * @brief Instantiate a worker that repeatedly carry out robot-scan matching. 67 | * The worker is implemented as a timer event that is invoked at a specific \c rate. 68 | * By specifying the argument \c spin_thread the timer event is invoked in a separate 69 | * thread and callback queue or otherwise it is called from the global callback queue (of the 70 | * node in which the plugin is used). 71 | * @param rate The rate that specifies how often the matching should be done. 72 | * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue) 73 | */ 74 | void startWorker(ros::Rate rate, bool spin_thread = false) 75 | { 76 | //setup 77 | 78 | 79 | // 80 | 81 | if (spin_thread_) 82 | { 83 | { 84 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 85 | need_to_terminate_ = true; 86 | } 87 | spin_thread_->join(); 88 | delete spin_thread_; 89 | } 90 | 91 | if (spin_thread) 92 | { 93 | ROS_DEBUG_NAMED("dynamicvoronoi", "Spinning up a thread for the dynamicvoronoi plugin"); 94 | need_to_terminate_ = false; 95 | spin_thread_ = new boost::thread(boost::bind(&BaseVoronoi::spinThread, this)); 96 | nh_.setCallbackQueue(&callback_queue_); 97 | } 98 | else 99 | { 100 | spin_thread_ = NULL; 101 | nh_.setCallbackQueue(ros::getGlobalCallbackQueue()); 102 | } 103 | 104 | worker_timer_ = nh_.createTimer(rate, &BaseVoronoi::workerCallback, this); 105 | } 106 | 107 | /** 108 | * @brief Stop the worker 109 | */ 110 | void stopWorker() 111 | { 112 | worker_timer_.stop(); 113 | if (spin_thread_) 114 | { 115 | { 116 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 117 | need_to_terminate_ = true; 118 | } 119 | spin_thread_->join(); 120 | delete spin_thread_; 121 | } 122 | } 123 | 124 | protected: 125 | 126 | /** 127 | * @brief Protected constructor that should be called by subclasses 128 | */ 129 | BaseVoronoi() : 130 | nh_("~base_voronoi"), spin_thread_(NULL), need_to_terminate_(false), voronoi_initialized_(false) 131 | {} 132 | 133 | /** 134 | * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) 135 | */ 136 | void spinThread() 137 | { 138 | while (nh_.ok()) 139 | { 140 | { 141 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 142 | if (need_to_terminate_) 143 | break; 144 | } 145 | callback_queue_.callAvailable(ros::WallDuration(0.1f)); 146 | } 147 | } 148 | 149 | /** 150 | * @brief The callback of the worker that performs the actual work 151 | */ 152 | void workerCallback(const ros::TimerEvent&) 153 | { 154 | 155 | compute(); 156 | } 157 | 158 | 159 | protected: 160 | ros::Timer worker_timer_; 161 | 162 | 163 | 164 | boost::mutex og_mutex_; 165 | nav_msgs::OccupancyGrid previous_grid_; 166 | nav_msgs::OccupancyGrid current_grid_; 167 | 168 | ros::Publisher voronoi_pub_; 169 | 170 | bool voronoi_initialized_; 171 | 172 | private: 173 | 174 | ros::NodeHandle nh_; 175 | boost::thread* spin_thread_; 176 | ros::CallbackQueue callback_queue_; 177 | boost::mutex terminate_mutex_; 178 | bool need_to_terminate_; 179 | 180 | }; 181 | }; 182 | #endif 183 | -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/voronoi_boost_plugin.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef VORONOI_COSTMAP_H_ 34 | #define VORONOI_COSTMAP_H_ 35 | 36 | #include 37 | 38 | 39 | #include 40 | #include 41 | 42 | 43 | #include 44 | 45 | namespace dynamicvoronoi 46 | { 47 | 48 | // sys/time.h 49 | timespec timespec_diff(timespec *a, timespec *b) { 50 | timespec result; 51 | result.tv_sec = a->tv_sec - b->tv_sec; 52 | result.tv_nsec = a->tv_nsec - b->tv_nsec; 53 | if (result.tv_nsec < 0) { 54 | --result.tv_sec; 55 | result.tv_nsec += 1000000000L; 56 | } 57 | return result; 58 | } 59 | 60 | 61 | class BoostPlugin : public dynamicvoronoi::BaseVoronoi 62 | { 63 | public: 64 | BoostPlugin() 65 | { 66 | initialized_ = false; 67 | voronoi_initialized_ = false; 68 | } 69 | 70 | ~BoostPlugin(); 71 | 72 | 73 | void compute(); 74 | 75 | /** 76 | * @brief Initializes the matcher plugin 77 | * @param nh ROS node handle 78 | * @param costmap_topic topic of thte occupancy grid of the local costmap 79 | */ 80 | void initialize(ros::NodeHandle nh); 81 | 82 | void occupancyGridCB(const nav_msgs::OccupancyGrid::ConstPtr& msg); 83 | 84 | void setCostmap2D(costmap_2d::Costmap2D *costmap, std::string global_frame_id); 85 | 86 | void setCostmapTopic(std::string costmap_topic, int sizeX, int sizeY, double resolution); 87 | 88 | teb_local_planner::HcGraph getGraph(); 89 | 90 | std::vector >> getPaths_world(); 91 | 92 | private: 93 | 94 | 95 | void DepthFirst(teb_local_planner::HcGraph& g, std::vector& visited, bool*& visited_array, const teb_local_planner::HcGraphVertexType& goal, int depth); 96 | 97 | ros::NodeHandle nh_; 98 | 99 | bool initialized_; //!< Keeps track about the correct initialization of this class 100 | 101 | ros::Subscriber occupancyGrid_sub_; 102 | bool useCostmap_; 103 | 104 | 105 | 106 | std::string global_frame_; 107 | 108 | BoostVoronoi voronoi_; 109 | 110 | std::vector >> paths_world; 111 | }; 112 | }; 113 | 114 | 115 | 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /dynamicvoronoi/include/dynamicvoronoi/voronoi_grid_plugin.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef VORONOI_GRID_H_ 34 | #define VORONOI_GRID_H_ 35 | #include 36 | 37 | // dynamic reconfigure 38 | //#include 39 | //#include 40 | #include 41 | 42 | 43 | namespace dynamicvoronoi 44 | { 45 | class GridPlugin : public dynamicvoronoi::BaseVoronoi 46 | { 47 | public: 48 | GridPlugin() : voronoi_() 49 | { 50 | initialized_ = false; 51 | } 52 | 53 | 54 | /** 55 | * @brief Initializes the matcher plugin 56 | * @param nh ROS node handle 57 | * @param costmap_topic topic of thte occupancy grid of the local costmap 58 | */ 59 | void initialize(ros::NodeHandle nh); 60 | 61 | void setCostmapTopic(std::string costmap_topic, int sizeX, int sizeY, double resolution); 62 | 63 | void occupancyGridCB(const nav_msgs::OccupancyGrid::ConstPtr& msg); 64 | 65 | void compute(); 66 | 67 | 68 | private: 69 | 70 | ros::NodeHandle nh_; 71 | 72 | bool initialized_; //!< Keeps track about the correct initialization of this class 73 | 74 | std::string costmap_topic_; 75 | 76 | ros::Subscriber occupancyGrid_sub_; //!< Subscriber 77 | 78 | ros::Publisher paths_pub_; 79 | 80 | DynamicVoronoi voronoi_; 81 | 82 | }; 83 | 84 | 85 | }; 86 | #endif 87 | -------------------------------------------------------------------------------- /dynamicvoronoi/launch/dynamicvoronoi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /dynamicvoronoi/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynamicvoronoi 4 | 0.0.0 5 | 6 | 7 | 8 | 9 |

10 | The first plugin using dynamic voronoi project by B. Lau. The project is modified for rolling window local costmap. 11 |

12 |

13 | The stack provides software to compute and update Euclidean distance maps (DM) and Euclidean Voronoi diagrams (GVD) on 2D grid maps. 14 |

15 |

16 | Details on the algorithms can be found in the corresponding paper. Please cite the paper if you use it for scientific work:
17 | B. Lau, C. Sprunk and W. Burgard, Improved Updating of Euclidean Distance Maps and Voronoi Diagrams, IEEE Intl. Conf. on Intelligent Robots and Systems (IROS), Taipei, Taiwan, 2010.
18 | See also http://www.informatik.uni-freiburg.de/~lau/dynamicvoronoi 19 |

20 |

21 | The second plugin generate topological voronoi graph using Boost library (with openCV for preprocessing). 22 |

23 | 24 | 25 | 26 | 27 | 28 | 29 | op 30 | Boris Lau, Christoph Sprunk, Wolfram Burgard 31 | Yiu Ming Chung 32 | 33 | 34 | 35 | 36 | BSD 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | catkin 63 | roscpp 64 | pluginlib 65 | costmap_2d 66 | pluginlib 67 | costmap_2d 68 | roscpp 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /dynamicvoronoi/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Extract GVD from occupancy grid published by the local costmap 4 | 5 | 6 | Extract GVD from Costmap2D object 7 | 8 | 9 | -------------------------------------------------------------------------------- /dynamicvoronoi/src/boost_voronoi.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | namespace dynamicvoronoi{ 36 | 37 | BoostVoronoi::BoostVoronoi() { 38 | //some initialisation 39 | 40 | og_mutex_ = new boost::mutex(); 41 | graph_mutex_ = new boost::mutex(); 42 | 43 | cost_translation_table_ = new char[256]; 44 | 45 | // special values: 46 | cost_translation_table_[0] = 0; // NO obstacle 47 | cost_translation_table_[253] = 99; // INSCRIBED obstacle 48 | cost_translation_table_[254] = 100; // LETHAL obstacle 49 | cost_translation_table_[255] = -1; // UNKNOWN 50 | 51 | // regular cost values scale the range 1 to 252 (inclusive) to fit 52 | // into 1 to 98 (inclusive). 53 | for (int i = 1; i < 253; i++) 54 | { 55 | cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251); 56 | } 57 | 58 | directCostmap_ = false; 59 | voronoi_initialized_ = true; 60 | } 61 | 62 | BoostVoronoi::~BoostVoronoi() { 63 | //some initialisation 64 | 65 | vd.clear(); 66 | 67 | delete[] cost_translation_table_; 68 | } 69 | 70 | boost::mutex* BoostVoronoi::getMutex() 71 | { 72 | return graph_mutex_; 73 | } 74 | 75 | boost::mutex* BoostVoronoi::getGridMutex() 76 | { 77 | return og_mutex_; 78 | } 79 | 80 | void BoostVoronoi::setCostmap2D(costmap_2d::Costmap2D *costmap, std::string global_frame_id) 81 | { 82 | global_frame_ = global_frame_id; 83 | 84 | if (!costmap) 85 | return; 86 | 87 | costmap_ = costmap; 88 | 89 | current_grid_ = boost::make_shared(); 90 | 91 | directCostmap_ = true; 92 | } 93 | 94 | void BoostVoronoi::disableCostmap2D() 95 | { 96 | costmap_ = NULL; 97 | directCostmap_ = false; 98 | } 99 | 100 | void BoostVoronoi::setOccupancyGrid(nav_msgs::OccupancyGrid::Ptr grid) 101 | { 102 | current_grid_ = grid; 103 | 104 | // change of map parameters is allowed 105 | sizeX_ = current_grid_->info.width; 106 | sizeY_ = current_grid_->info.height; 107 | resolution_ = current_grid_->info.resolution; 108 | origin_.x() = current_grid_->info.origin.position.x; 109 | origin_.y() = current_grid_->info.origin.position.y; 110 | } 111 | 112 | void BoostVoronoi::updateGridFromCostmap() 113 | { 114 | 115 | if (voronoi_initialized_ && directCostmap_) 116 | { 117 | // refer to costmap_2d_publisher.cpp 118 | // https://github.com/ros-planning/navigation/blob/noetic-devel/costmap_2d/src/costmap_2d_publisher.cpp 119 | 120 | costmap_2d::Costmap2D::mutex_t::scoped_lock l(*costmap_->getMutex()); 121 | 122 | // may have external visualisation 123 | boost::mutex::scoped_lock l_og(*og_mutex_); 124 | 125 | double resolution = costmap_->getResolution(); 126 | 127 | current_grid_->header.frame_id = global_frame_; 128 | current_grid_->header.stamp = ros::Time::now(); 129 | current_grid_->info.resolution = resolution; 130 | 131 | current_grid_->info.width = costmap_->getSizeInCellsX(); 132 | current_grid_->info.height = costmap_->getSizeInCellsY(); 133 | 134 | double wx, wy; 135 | costmap_->mapToWorld(0, 0, wx, wy); 136 | current_grid_->info.origin.position.x = wx - resolution / 2; 137 | current_grid_->info.origin.position.y = wy - resolution / 2; 138 | current_grid_->info.origin.position.z = 0.0; 139 | current_grid_->info.origin.orientation.w = 1.0; 140 | //saved_origin_x_ = costmap_->getOriginX(); 141 | //saved_origin_y_ = costmap_->getOriginY(); 142 | 143 | current_grid_->data.resize(current_grid_->info.width * current_grid_->info.height); 144 | 145 | unsigned char* data = costmap_->getCharMap(); 146 | for (unsigned int i = 0; i < current_grid_->data.size(); i++) 147 | { 148 | current_grid_->data[i] = cost_translation_table_[ data[ i ]]; 149 | } 150 | 151 | // change of map parameters is allowed 152 | sizeX_ = current_grid_->info.width; 153 | sizeY_ = current_grid_->info.height; 154 | resolution_ = current_grid_->info.resolution; 155 | origin_.x() = current_grid_->info.origin.position.x; 156 | origin_.y() = current_grid_->info.origin.position.y; 157 | 158 | } 159 | } 160 | 161 | 162 | 163 | void BoostVoronoi::updateGraph() 164 | { 165 | 166 | try 167 | { 168 | 169 | boost::mutex::scoped_lock l(*graph_mutex_); 170 | 171 | graph_.clear(); 172 | vd.clear(); 173 | 174 | // compute only after the first valid occupancy grid is received 175 | if (voronoi_initialized_ && current_grid_->header.stamp != ros::Time() && current_grid_->info.width != 0 && current_grid_->info.height != 0) 176 | { 177 | 178 | // get a copy instead of using the original occupancy grid in the planning rate is faster than grid publishing rate 179 | image_ = cv::Mat(sizeY_,sizeX_, CV_8UC1); 180 | std::memcpy(image_.data, ¤t_grid_->data[0], sizeY_*sizeX_*sizeof(int8_t)); 181 | 182 | // opencv pre processing 183 | 184 | // if we add an obstacle border later, a white border must be added first to prevent intersection of segments 185 | cv::Rect border(cv::Point(0, 0), image_.size()); 186 | cv::rectangle(image_, border, cv::Scalar(0,0,0), 10); 187 | 188 | cv::threshold( image_, image_, 50, 100, cv::THRESH_BINARY ); // threshold non-lethal cost to zero 189 | // remove noise 190 | cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(4,4)); 191 | cv::morphologyEx(image_, image_, cv::MORPH_OPEN, kernel); 192 | cv::morphologyEx(image_, image_, cv::MORPH_CLOSE, kernel); 193 | 194 | std::vector > contours; 195 | std::vector hierarchy; 196 | cv::findContours( image_, contours, hierarchy, cv::RETR_EXTERNAL , cv::CHAIN_APPROX_SIMPLE ); 197 | 198 | std::vector > appr_contours; 199 | for( size_t i = 0; i< contours.size(); i++ ) 200 | { 201 | double epsilon = 0.01*cv::arcLength(contours[i],true); 202 | std::vector approximate; 203 | cv::approxPolyDP(contours[i],approximate,epsilon,true); 204 | appr_contours.push_back(approximate); 205 | } 206 | 207 | 208 | // mutex due to external publishing 209 | boost::mutex::scoped_lock l(*og_mutex_); 210 | 211 | output.header = current_grid_->header; 212 | output.info.height = sizeY_; 213 | output.info.width = sizeX_; 214 | output.info.resolution = resolution_; 215 | output.info.origin = current_grid_->info.origin; 216 | output.info.origin.position.z = -0.01; 217 | output.data.resize(sizeY_ * sizeX_); 218 | 219 | cv::Mat output_image(int(sizeY_), int(sizeX_), CV_8UC1, &output.data[0]); 220 | output_image.setTo(cv::Scalar(0,0,0)); 221 | 222 | for( size_t i = 0; i< appr_contours.size(); i++ ) 223 | { 224 | cv::drawContours( output_image, appr_contours, (int)i, cv::Scalar(30,0,0), cv::FILLED, cv::LINE_4, hierarchy, 0 ); 225 | } 226 | 227 | // opencv post processing for discretization error of grid comparison 228 | //kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3)); 229 | //cv::erode(output_image, output_image, kernel); 230 | 231 | 232 | std::vector points; 233 | std::vector segments; 234 | 235 | for( size_t i = 0; i< appr_contours.size(); i++ ) 236 | { 237 | for (auto it_pt = appr_contours[i].begin(); it_pt != std::prev(appr_contours[i].end()); it_pt++) 238 | { 239 | segments.push_back(Segment(it_pt->x, it_pt->y, std::next(it_pt)->x, std::next(it_pt)->y)); 240 | } 241 | segments.push_back(Segment(std::prev(appr_contours[i].end())->x, std::prev(appr_contours[i].end())->y, appr_contours[i].begin()->x, appr_contours[i].begin()->y)); 242 | } 243 | 244 | 245 | // Add a boundary to the costmap to avoid infinite edges 246 | segments.push_back(Segment(0, 0, sizeX_-1, 0)); 247 | segments.push_back(Segment(sizeX_-1, 0, sizeX_-1, sizeY_-1)); 248 | segments.push_back(Segment(sizeX_-1, sizeY_-1, 0, sizeY_-1)); 249 | segments.push_back(Segment(0, sizeY_-1, 0, 0)); 250 | 251 | 252 | boost::polygon::construct_voronoi(points.begin(), points.end(), segments.begin(), segments.end(), &vd); 253 | 254 | std::unordered_map vertices_map; 255 | std::vector::const_vertex_iterator> voronoi_vertices; 256 | 257 | int count = 0; 258 | for (boost::polygon::voronoi_diagram::const_vertex_iterator it = vd.vertices().begin(); it != vd.vertices().end(); ++it) 259 | { 260 | // degenerate vertice are handled by hashtable 261 | if ( it->x()>0 && it->x()y()>0 && it->y()(lround(it->y()), lround(it->x())) < 10 ) 267 | { 268 | teb_local_planner::HcGraphVertexType v = boost::add_vertex(graph_); 269 | graph_[v].pos.x() = it->x() * resolution_ + origin_.x(); 270 | graph_[v].pos.y() = it->y() * resolution_ + origin_.y(); 271 | vertices_map.emplace(Eigen::Vector2d(it->x(), it->y()), v); 272 | voronoi_vertices.push_back(it); 273 | count++; 274 | } 275 | 276 | } 277 | } 278 | 279 | for (auto it = voronoi_vertices.begin(); it != voronoi_vertices.end(); it++) 280 | { 281 | const boost::polygon::voronoi_diagram::edge_type *edge = (*it)->incident_edge(); 282 | do 283 | { 284 | if (edge->is_primary()) 285 | { 286 | if (edge->is_finite()){ 287 | 288 | // confirm vertices exist 289 | auto v1 = vertices_map.find(Eigen::Vector2d(edge->vertex0()->x(), edge->vertex0()->y())); 290 | auto v2 = vertices_map.find(Eigen::Vector2d(edge->vertex1()->x(), edge->vertex1()->y())); 291 | 292 | if (v1 != vertices_map.end() && v2 != vertices_map.end()) 293 | { 294 | if ( output_image.at(lround(edge->vertex0()->y()), lround(edge->vertex0()->x())) < 10 && output_image.at(lround(edge->vertex1()->y()), lround(edge->vertex1()->x())) < 10) 295 | boost::add_edge( v1->second, v2->second, graph_); 296 | } 297 | 298 | } 299 | } 300 | edge = edge->rot_next(); 301 | } while (edge != (*it)->incident_edge()); 302 | } 303 | 304 | // traverse the boost graph again to ensure correctness 305 | 306 | teb_local_planner::HcGraphEdgeIterator it_i, end_i; 307 | for (boost::tie(it_i,end_i) = boost::edges(graph_); it_i!=end_i; ++it_i) 308 | { 309 | int x1, y1, x2, y2; 310 | 311 | x1 = (graph_[source(*it_i, graph_)].pos.x() - origin_.x()) / resolution_; 312 | y1 = (graph_[source(*it_i, graph_)].pos.y() - origin_.y()) / resolution_; 313 | x2 = (graph_[target(*it_i, graph_)].pos.x() - origin_.x()) / resolution_; 314 | y2 = (graph_[target(*it_i, graph_)].pos.y() - origin_.y()) / resolution_; 315 | 316 | cv::line(output_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(100,0,0), 1, cv::LINE_8); 317 | } 318 | 319 | 320 | 321 | //voronoi_pub_.publish(output); 322 | 323 | //image_.release(); 324 | //output_image.release(); 325 | 326 | 327 | } 328 | 329 | 330 | } 331 | catch(const std::exception& ex) 332 | { 333 | ROS_ERROR_STREAM("Un-handled Error: " << ex.what() << "."); 334 | } 335 | 336 | 337 | 338 | } 339 | 340 | 341 | boost::optional> BoostVoronoi::addStartAndGoal(const Eigen::Vector2d& start, const Eigen::Vector2d& goal) 342 | { 343 | // as the number and the dimension of points are both small, navie search seems not a bad idea 344 | boost::mutex::scoped_lock l(*graph_mutex_); 345 | 346 | if (boost::num_vertices(graph_) > 0) 347 | { 348 | teb_local_planner::HcGraphVertexIterator it_i, end_i; 349 | boost::tie(it_i,end_i) = boost::vertices(graph_); 350 | 351 | double min_start_dist = INFINITY; 352 | teb_local_planner::HcGraphVertexIterator min_start_vertex = end_i; 353 | double min_goal_dist = INFINITY; 354 | teb_local_planner::HcGraphVertexIterator min_goal_vertex = end_i; 355 | 356 | while (it_i!=end_i) 357 | { 358 | double start_test_dist = (graph_[*it_i].pos - start).norm(); 359 | double goal_test_dist = (graph_[*it_i].pos - goal).norm(); 360 | if ( start_test_dist <= min_start_dist ) 361 | { 362 | min_start_dist = start_test_dist; 363 | min_start_vertex = it_i; 364 | } 365 | if ( goal_test_dist <= min_goal_dist ) 366 | { 367 | min_goal_dist = goal_test_dist; 368 | min_goal_vertex = it_i; 369 | } 370 | 371 | ++it_i; 372 | } 373 | 374 | teb_local_planner::HcGraphVertexType v_s = boost::add_vertex(graph_); 375 | graph_[v_s].pos = start; 376 | 377 | teb_local_planner::HcGraphVertexType v_g = boost::add_vertex(graph_); 378 | graph_[v_g].pos = goal; 379 | 380 | boost::add_edge(v_s, *min_start_vertex, graph_); 381 | boost::add_edge(*min_goal_vertex, v_g, graph_); 382 | 383 | return std::make_pair(v_s, v_g); 384 | } 385 | 386 | return boost::none; 387 | } 388 | 389 | boost::shared_ptr BoostVoronoi::getGraph() 390 | { 391 | return boost::make_shared (graph_) ; 392 | } 393 | 394 | const nav_msgs::OccupancyGrid& BoostVoronoi::getVisualisation() 395 | { 396 | return output; 397 | } 398 | 399 | void BoostVoronoi::addCircularObstacle(double x, double y, double r) 400 | { 401 | cv::Mat image(sizeY_, sizeX_, CV_8UC1, ¤t_grid_->data[0]); 402 | 403 | cv::circle(image, cv::Point((x-origin_.x())/resolution_, (y-origin_.y())/resolution_), lround(r/resolution_), cv::Scalar(100,0,0), cv::FILLED, cv::LINE_8 ); 404 | 405 | } 406 | 407 | void BoostVoronoi::addLineObstacle(const Eigen::Vector2d& pt1, const Eigen::Vector2d& pt2, double t) 408 | { 409 | double x1 = pt1.x(); 410 | double y1 = pt1.y(); 411 | double x2 = pt2.x(); 412 | double y2 = pt2.y(); 413 | 414 | cv::Mat image(int(current_grid_->info.height), int(current_grid_->info.width), CV_8UC1, ¤t_grid_->data[0]); 415 | 416 | cv::line( image, cv::Point((x1-origin_.x())/resolution_, (y1-origin_.y())/resolution_), 417 | cv::Point((x2-origin_.x())/resolution_, (y2-origin_.y())/resolution_), cv::Scalar(100,0,0), lround(t/resolution_), cv::LINE_8 ); 418 | 419 | } 420 | 421 | void BoostVoronoi::addPillObstacle(const Eigen::Vector2d& pt1, const Eigen::Vector2d& pt2, double r) 422 | { 423 | double x1 = pt1.x(); 424 | double y1 = pt1.y(); 425 | double x2 = pt2.x(); 426 | double y2 = pt2.y(); 427 | 428 | cv::Mat image(int(current_grid_->info.height), int(current_grid_->info.width), CV_8UC1, ¤t_grid_->data[0]); 429 | 430 | Eigen::Vector2d dir(x2-x1,y2-y1); 431 | 432 | Eigen::Vector2d normal = Eigen::Vector2d(-dir.y(), dir.x()).normalized(); 433 | 434 | std::vector points{ cv::Point( (x1+r*normal.x()-origin_.x())/resolution_, (y1+r*normal.y()-origin_.y())/resolution_), cv::Point((x2+r*normal.x()-origin_.x())/resolution_, (y2+r*normal.y()-origin_.y())/resolution_), 435 | cv::Point( (x2-r*normal.x()-origin_.x())/resolution_, (y2-r*normal.y()-origin_.y())/resolution_), cv::Point((x1-r*normal.x()-origin_.x())/resolution_, (y1-r*normal.y()-origin_.y())/resolution_)}; 436 | 437 | cv::fillConvexPoly(image, points, cv::Scalar(100,0,0), cv::LINE_8 ); 438 | 439 | // or should we draw 2 half ellipses instead of full circles? 440 | cv::circle(image, cv::Point((x1-origin_.x())/resolution_, (y1-origin_.y())/resolution_), r/resolution_, cv::Scalar(100,0,0), cv::FILLED, cv::LINE_8 ); 441 | cv::circle(image, cv::Point((x2-origin_.x())/resolution_, (y2-origin_.y())/resolution_), r/resolution_, cv::Scalar(100,0,0), cv::FILLED, cv::LINE_8 ); 442 | 443 | } 444 | 445 | void BoostVoronoi::addPolygonObstacle(const std::vector>& points) 446 | { 447 | cv::Mat image(int(current_grid_->info.height), int(current_grid_->info.width), CV_8UC1, ¤t_grid_->data[0]); 448 | 449 | std::vector cv_points; 450 | for (auto it = points.begin(); it != points.end(); it++) 451 | { 452 | cv_points.push_back(cv::Point( (it->x()-origin_.x())/resolution_, (it->y()-origin_.y())/resolution_)); 453 | } 454 | cv::fillConvexPoly(image, cv_points, cv::Scalar(100,0,0), cv::FILLED, cv::LINE_8 ); 455 | 456 | } 457 | 458 | 459 | 460 | } // namespace dynamicvoronoi -------------------------------------------------------------------------------- /dynamicvoronoi/src/bucketedqueue.cpp: -------------------------------------------------------------------------------- 1 | // for comparison only 2 | // source: http://www2.informatik.uni-freiburg.de/~lau/dynamicvoronoi/ 3 | // BSD licence 4 | 5 | 6 | #include 7 | 8 | #include "limits.h" 9 | #include 10 | #include 11 | 12 | std::vector BucketPrioQueue::sqrIndices; 13 | int BucketPrioQueue::numBuckets; 14 | 15 | 16 | BucketPrioQueue::BucketPrioQueue() { 17 | // make sure the index array is created 18 | if (sqrIndices.size()==0) initSqrIndices(); 19 | nextBucket = INT_MAX; 20 | 21 | // now create the buckets 22 | // buckets = std::vector >(numBuckets); 23 | buckets = std::vector >(numBuckets); 24 | 25 | // reset element counter 26 | count = 0; 27 | } 28 | 29 | bool BucketPrioQueue::empty() { 30 | return (count==0); 31 | } 32 | 33 | 34 | void BucketPrioQueue::push(int prio, INTPOINT t) { 35 | if (prio>=(int)sqrIndices.size()) { 36 | fprintf(stderr, "error: priority %d is not a valid squared distance x*x+y*y, or x>MAXDIST or y>MAXDIST.\n", prio); 37 | exit(-1); 38 | } 39 | int id = sqrIndices[prio]; 40 | if (id<0) { 41 | fprintf(stderr, "error: priority %d is not a valid squared distance x*x+y*y, or x>MAXDIST or y>MAXDIST.\n", prio); 42 | exit(-1); 43 | } 44 | buckets[id].push(t); 45 | // printf("pushing %d with prio %d into %d. Next: %d\n", t.x, prio, id, nextBucket); 46 | if (id0); 54 | // printf("next: %d\n", nextBucket); 55 | for (i = nextBucket; i<(int)buckets.size(); i++) { 56 | if (!buckets[i].empty()) break; 57 | } 58 | assert(i<(int)buckets.size()); 59 | nextBucket = i; 60 | // printf("pop new next %d\n", nextBucket); 61 | count--; 62 | INTPOINT p = buckets[i].front(); 63 | buckets[i].pop(); 64 | return p; 65 | } 66 | 67 | 68 | void BucketPrioQueue::initSqrIndices() { 69 | // std::cout << "BUCKETQUEUE Starting to build the index array...\n"; 70 | // std::set sqrNumbers; 71 | 72 | sqrIndices = std::vector(2*MAXDIST*MAXDIST+1, -1); 73 | 74 | int count=0; 75 | for (int x=0; x<=MAXDIST; x++) { 76 | for (int y=0; y<=x; y++) { 77 | int sqr = x*x+y*y; 78 | sqrIndices[sqr] = count++; 79 | } 80 | } 81 | numBuckets = count; 82 | // std::cout << "BUCKETQUEUE Done with building the index arrays.\n"; 83 | } 84 | -------------------------------------------------------------------------------- /dynamicvoronoi/src/dynamicvoronoi_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | class VoronoiProcess 39 | { 40 | public: 41 | VoronoiProcess(ros::NodeHandle* nodehandle): n_(*nodehandle), loader_("dynamicvoronoi", "dynamicvoronoi::BaseVoronoi") 42 | { 43 | 44 | //n_ = ros::NodeHandle("~"); 45 | 46 | int rate = 20; 47 | 48 | try 49 | { 50 | generator_ = loader_.createInstance("dynamicvoronoi::BoostPlugin"); 51 | } 52 | catch(const pluginlib::PluginlibException& ex) 53 | { 54 | ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); 55 | ros::shutdown(); 56 | } 57 | 58 | ROS_INFO_STREAM("Standalone voronoi generator:" << "dynamicvoronoi::BoostPlugin" << " loaded."); 59 | 60 | 61 | std::string node_ns = ros::this_node::getNamespace(); 62 | ros::NodeHandle costmap_nh(node_ns+"/move_base/local_costmap"); 63 | double width = 10; 64 | costmap_nh.param("width", width, width); 65 | double height = 10; 66 | costmap_nh.param("height", height, height); 67 | double resolution = 0.05; 68 | costmap_nh.param("resolution", resolution, resolution); 69 | unsigned int width_cell = (unsigned int) (width / resolution); 70 | unsigned int height_cell = (unsigned int)(height / resolution); 71 | 72 | 73 | if (generator_) 74 | { 75 | generator_->initialize(n_); 76 | generator_->setCostmapTopic(node_ns+"/move_base/local_costmap/costmap", width_cell, height_cell, resolution); 77 | generator_->startWorker(ros::Rate(double(rate)), true); 78 | } 79 | 80 | } 81 | 82 | 83 | private: 84 | pluginlib::ClassLoader loader_; 85 | boost::shared_ptr generator_; 86 | 87 | ros::NodeHandle n_; 88 | }; 89 | 90 | int main(int argc, char** argv) 91 | { 92 | ros::init(argc, argv, "voronoi_node"); 93 | 94 | ros::NodeHandle nh; 95 | 96 | VoronoiProcess process(&nh); 97 | 98 | ros::spin(); 99 | return 0; 100 | } -------------------------------------------------------------------------------- /dynamicvoronoi/src/voronoi_boost_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | 36 | #include 37 | 38 | 39 | PLUGINLIB_EXPORT_CLASS(dynamicvoronoi::BoostPlugin, dynamicvoronoi::BaseVoronoi) 40 | 41 | namespace dynamicvoronoi 42 | { 43 | 44 | BoostPlugin::~BoostPlugin() 45 | { 46 | 47 | } 48 | 49 | void BoostPlugin::initialize(ros::NodeHandle nh) 50 | { 51 | if(!initialized_) 52 | { 53 | 54 | std::string ns = ""; 55 | { 56 | using namespace ros; 57 | ns = this_node::getNamespace(); 58 | } 59 | 60 | voronoi_pub_ = nh.advertise(ns+"/voronoi",1); 61 | 62 | previous_grid_ = nav_msgs::OccupancyGrid(); 63 | 64 | 65 | initialized_ = true; 66 | ROS_DEBUG("voronoi plugin initialized."); 67 | } 68 | else 69 | { 70 | ROS_WARN("voronoi local plugin has already been initialized, doing nothing."); 71 | } 72 | } 73 | 74 | void BoostPlugin::occupancyGridCB(const nav_msgs::OccupancyGrid::ConstPtr& msg) 75 | { 76 | 77 | boost::mutex::scoped_lock l(og_mutex_); 78 | current_grid_ = * msg; 79 | 80 | } 81 | 82 | void BoostPlugin::setCostmap2D(costmap_2d::Costmap2D *costmap, std::string global_frame_id) 83 | { 84 | global_frame_ = global_frame_id; 85 | 86 | if (!costmap) 87 | return; 88 | 89 | voronoi_.setCostmap2D(costmap, global_frame_id); 90 | 91 | useCostmap_ = true; 92 | voronoi_initialized_ = true; 93 | 94 | } 95 | 96 | void BoostPlugin::setCostmapTopic(std::string costmap_topic, int sizeX, int sizeY, double resolution) 97 | { 98 | occupancyGrid_sub_ = nh_.subscribe(costmap_topic, 1, &BoostPlugin::occupancyGridCB, this); 99 | 100 | voronoi_.disableCostmap2D(); 101 | useCostmap_ = false; 102 | 103 | voronoi_initialized_ = true; 104 | } 105 | 106 | 107 | void BoostPlugin::compute() 108 | { 109 | 110 | timespec time1; 111 | timespec time2; 112 | clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1); 113 | if(useCostmap_) 114 | { 115 | voronoi_.updateGridFromCostmap(); 116 | } 117 | else 118 | { 119 | voronoi_.setOccupancyGrid(boost::make_shared(current_grid_)); 120 | } 121 | 122 | 123 | //voronoi_.addCircularObstacle(25,25, 5); 124 | //voronoi_.addPillObstacle(Eigen::Vector2d(50,50), Eigen::Vector2d(75, 75), 5); 125 | 126 | 127 | voronoi_.updateGraph(); 128 | 129 | voronoi_pub_.publish(voronoi_.getVisualisation()); 130 | 131 | Eigen::Vector2d start(current_grid_.info.origin.position.x+1, current_grid_.info.origin.position.y+1); 132 | Eigen::Vector2d goal = start + Eigen::Vector2d(current_grid_.info.width/current_grid_.info.resolution*2/3, current_grid_.info.height/current_grid_.info.resolution*2/3); 133 | 134 | boost::optional> start_goal 135 | = voronoi_.addStartAndGoal(start, goal); 136 | 137 | int count=0; 138 | int* countPtr = &count; 139 | 140 | if (start_goal) 141 | { 142 | teb_local_planner::HcGraphVertexType start_vtx, goal_vtx; 143 | start_vtx = (*start_goal).first; 144 | goal_vtx = (*start_goal).second; 145 | 146 | teb_local_planner::HcGraph graph = *voronoi_.getGraph(); 147 | std::vector visited; 148 | visited.push_back(start_vtx); 149 | bool* visited_array = new bool[boost::num_vertices(graph)](); 150 | visited_array[start_vtx] = true; 151 | 152 | paths_world.clear(); 153 | 154 | DepthFirst( graph, visited, visited_array, goal_vtx, 500); 155 | 156 | delete[] visited_array; 157 | visited_array = NULL; 158 | 159 | } 160 | 161 | 162 | clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2); 163 | timespec time_diff = timespec_diff(&time2, &time1); 164 | double run_time = (time_diff.tv_nsec)*1e-6; // milliseconds 165 | 166 | ROS_INFO_STREAM("Total runtime: "<& visited, bool*& visited_array, const teb_local_planner::HcGraphVertexType& goal, int depth) 174 | { 175 | // see http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ for details on finding all simple paths 176 | 177 | //if ((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes) 178 | // return; // We do not need to search for further possible alternative homotopy classes. 179 | 180 | teb_local_planner::HcGraphVertexType back = visited.back(); 181 | 182 | /// Examine adjacent nodes 183 | teb_local_planner::HcGraphAdjecencyIterator it, end; 184 | for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it) 185 | { 186 | //if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() ) 187 | // constant time access 188 | if ( visited_array[*it] ) 189 | continue; // already visited 190 | 191 | if ( *it == goal ) // goal reached 192 | { 193 | visited.push_back(*it); 194 | 195 | // Add new TEB, if this path belongs to a new homotopy class 196 | //hcp_->addAndInitNewTeb(visited.begin(), visited.end(), boost::bind(getVector2dFromHcGraph, _1, boost::cref(graph_)), 197 | // start_orientation, goal_orientation, start_velocity); 198 | 199 | std::vector> path_world; 200 | 201 | for (auto it_v = visited.begin(); it_v != visited.end(); it_v++) 202 | { 203 | path_world.push_back( std::pair(g[*it_v].pos.x(), g[*it_v].pos.y()) ); 204 | } 205 | 206 | paths_world.push_back(path_world); 207 | 208 | visited.pop_back(); 209 | break; 210 | } 211 | } 212 | 213 | if (depth > 0) 214 | { 215 | /// Recursion for all adjacent vertices 216 | for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it) 217 | { 218 | if ( visited_array[*it] || *it == goal) 219 | continue; // already visited || goal reached 220 | 221 | 222 | visited.push_back(*it); 223 | visited_array[*it] = true; 224 | 225 | // recursion step 226 | DepthFirst(g, visited, visited_array, goal, depth-1); 227 | 228 | visited.pop_back(); 229 | visited_array[*it] = false; 230 | } 231 | } 232 | } 233 | 234 | 235 | std::vector >> BoostPlugin::getPaths_world() 236 | { 237 | return paths_world; 238 | } 239 | 240 | 241 | } 242 | 243 | -------------------------------------------------------------------------------- /dynamicvoronoi/src/voronoi_grid_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | 39 | PLUGINLIB_EXPORT_CLASS(dynamicvoronoi::GridPlugin, dynamicvoronoi::BaseVoronoi) 40 | 41 | namespace dynamicvoronoi 42 | { 43 | 44 | void GridPlugin::initialize(ros::NodeHandle nh) 45 | { 46 | if(!initialized_) 47 | { 48 | nh_ = nh; 49 | std::string ns = ""; 50 | { 51 | using namespace ros; 52 | ns = this_node::getNamespace(); 53 | } 54 | 55 | voronoi_pub_ = nh_.advertise(ns+"/voronoi",1); 56 | paths_pub_ = nh_.advertise(ns+"/voronoi_paths", 5); 57 | 58 | previous_grid_ = nav_msgs::OccupancyGrid(); 59 | 60 | 61 | initialized_ = true; 62 | ROS_DEBUG("voronoi plugin initialized."); 63 | } 64 | else 65 | { 66 | ROS_WARN("voronoi local plugin has already been initialized, doing nothing."); 67 | } 68 | } 69 | 70 | void GridPlugin::setCostmapTopic(std::string costmap_topic, int sizeX, int sizeY, double resolution) 71 | { 72 | occupancyGrid_sub_ = nh_.subscribe(costmap_topic, 1, &GridPlugin::occupancyGridCB, this); 73 | 74 | voronoi_.initializeEmpty(sizeX, sizeY, true); 75 | voronoi_initialized_ = true; 76 | } 77 | 78 | void GridPlugin::occupancyGridCB(const nav_msgs::OccupancyGrid::ConstPtr& msg) 79 | { 80 | 81 | boost::mutex::scoped_lock l(og_mutex_); 82 | current_grid_ = * msg; 83 | 84 | } 85 | 86 | void GridPlugin::compute() 87 | { 88 | 89 | boost::mutex::scoped_lock l(og_mutex_); 90 | 91 | if (voronoi_initialized_ && current_grid_.header.stamp != ros::Time()) 92 | { 93 | 94 | ROS_ASSERT_MSG(current_grid_.info.width == voronoi_.getSizeX() && current_grid_.info.height == voronoi_.getSizeY(), "Mismatched grid sizes."); 95 | 96 | if (current_grid_.info.width == voronoi_.getSizeX() && current_grid_.info.height == voronoi_.getSizeY()) 97 | { 98 | 99 | double resolution = current_grid_.info.resolution; 100 | int dis_x_cell = int(round((current_grid_.info.origin.position.x - previous_grid_.info.origin.position.x) / resolution)); 101 | int dis_y_cell = int(round((current_grid_.info.origin.position.y - previous_grid_.info.origin.position.y) / resolution)); 102 | 103 | int source_x = 0; 104 | int source_y = 0; 105 | int target_x = - dis_x_cell; 106 | int target_y = - dis_y_cell; 107 | int overlap_size_x = current_grid_.info.width + dis_x_cell; 108 | int overlap_size_y = current_grid_.info.height + dis_y_cell; 109 | if (dis_x_cell >= 0) 110 | { 111 | source_x = dis_x_cell; 112 | target_x = 0; 113 | overlap_size_x = current_grid_.info.width - dis_x_cell; 114 | } 115 | if (dis_y_cell >= 0) 116 | { 117 | source_y = dis_y_cell; 118 | target_y = 0; 119 | overlap_size_y = current_grid_.info.height - dis_y_cell; 120 | } 121 | 122 | 123 | if ( (dis_x_cell != 0 || dis_y_cell != 0) && abs(dis_x_cell) < voronoi_.getSizeX() && abs(dis_y_cell) < voronoi_.getSizeY() ) 124 | { 125 | voronoi_.move(source_x, source_y, target_x, target_y, overlap_size_x, overlap_size_y); 126 | } 127 | 128 | for (int x=0; x 50) 134 | { 135 | voronoi_.occupyCell(x,y); 136 | } 137 | else 138 | { 139 | voronoi_.clearCell(x,y); 140 | } 141 | } 142 | } 143 | 144 | 145 | voronoi_.update(); 146 | voronoi_.prune(); 147 | 148 | previous_grid_ = current_grid_; 149 | 150 | 151 | nav_msgs::OccupancyGrid output = current_grid_; 152 | 153 | 154 | for(int y = voronoi_.getSizeY()-1; y >=0; y--) 155 | { 156 | for(int x = 0; x >> paths_world = voronoi_.getPaths_world(); 183 | 184 | 185 | visualization_msgs::Marker line_strip; 186 | line_strip.header.frame_id = current_grid_.header.frame_id; 187 | line_strip.header.stamp = ros::Time::now(); 188 | line_strip.action = visualization_msgs::Marker::ADD; 189 | line_strip.pose.orientation.w = 1.0; 190 | 191 | line_strip.type = visualization_msgs::Marker::LINE_STRIP; 192 | line_strip.scale.x = 0.1; 193 | line_strip.color.a = 1.0; 194 | line_strip.color.b = 1.0; 195 | line_strip.lifetime = ros::Duration(0.15); 196 | 197 | int count = 0; 198 | 199 | for (auto it_path=paths_world.begin(); it_path!=paths_world.end(); it_path++) 200 | { 201 | 202 | count++; 203 | line_strip.ns = "path_"+ std::to_string(count); 204 | line_strip.points.clear(); 205 | 206 | for (auto it_pt=it_path->begin(); it_pt!=it_path->end(); it_pt++) 207 | { 208 | geometry_msgs::Point p; 209 | p.x = it_pt->first; 210 | p.y = it_pt->second; 211 | p.z = 0; 212 | line_strip.points.push_back(p); 213 | } 214 | paths_pub_.publish(line_strip); 215 | } 216 | 217 | 218 | 219 | } 220 | 221 | } 222 | 223 | } 224 | 225 | } -------------------------------------------------------------------------------- /msg_merger/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(msg_merger) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | pluginlib 11 | roscpp 12 | teb_local_planner 13 | ) 14 | 15 | ## Find Boost (headers only) 16 | find_package(Boost REQUIRED) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | INCLUDE_DIRS include 106 | LIBRARIES msg_merger 107 | CATKIN_DEPENDS pluginlib roscpp teb_local_planner 108 | DEPENDS Boost 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 118 | 119 | ## Declare a C++ library 120 | add_library(${PROJECT_NAME} 121 | src/msg_merger_plugin.cpp 122 | ) 123 | 124 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | add_executable(${PROJECT_NAME}_node src/msg_merger_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | target_link_libraries(${PROJECT_NAME}_node 148 | ${catkin_LIBRARIES} 149 | ) 150 | 151 | add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 176 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | ) 180 | 181 | ## Mark cpp header files for installation 182 | install(DIRECTORY include/${PROJECT_NAME}/ 183 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | FILES_MATCHING PATTERN "*.h" 185 | PATTERN ".svn" EXCLUDE 186 | ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | install(FILES plugins.xml 190 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_msg_merger.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) 204 | -------------------------------------------------------------------------------- /msg_merger/include/msg_merger/msg_merger_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef MSG_MERGER_BASE_H_ 34 | #define MSG_MERGER_BASE_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | namespace msg_merger_base 41 | { 42 | class BaseMerger 43 | { 44 | public: 45 | virtual void initialize(ros::NodeHandle nh) = 0; 46 | 47 | virtual ~BaseMerger(){stopWorker();} 48 | 49 | 50 | /** 51 | * @brief This method performs the actual work 52 | */ 53 | virtual void compute() = 0; 54 | virtual void compute2() = 0; 55 | 56 | /** 57 | * @brief Instantiate a worker that repeatedly carry out robot-scan matching. 58 | * The worker is implemented as a timer event that is invoked at a specific \c rate. 59 | * By specifying the argument \c spin_thread the timer event is invoked in a separate 60 | * thread and callback queue or otherwise it is called from the global callback queue (of the 61 | * node in which the plugin is used). 62 | * @param rate The rate that specifies how often the matching should be done. 63 | * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue) 64 | */ 65 | void startWorker(ros::Rate rate, bool spin_thread = false) 66 | { 67 | //setup 68 | 69 | 70 | // 71 | 72 | if (spin_thread_) 73 | { 74 | { 75 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 76 | need_to_terminate_ = true; 77 | } 78 | spin_thread_->join(); 79 | delete spin_thread_; 80 | } 81 | 82 | if (spin_thread) 83 | { 84 | ROS_DEBUG_NAMED("msg_merger", "Spinning up a thread for the msg_merger plugin"); 85 | need_to_terminate_ = false; 86 | spin_thread_ = new boost::thread(boost::bind(&BaseMerger::spinThread, this)); 87 | nh_.setCallbackQueue(&callback_queue_); 88 | } 89 | else 90 | { 91 | spin_thread_ = NULL; 92 | nh_.setCallbackQueue(ros::getGlobalCallbackQueue()); 93 | } 94 | 95 | worker_timer_ = nh_.createTimer(1, &BaseMerger::workerCallback, this); 96 | worker_timer2_ = nh_.createTimer(rate, &BaseMerger::workerCallback2, this); 97 | } 98 | 99 | /** 100 | * @brief Stop the worker 101 | */ 102 | void stopWorker() 103 | { 104 | worker_timer_.stop(); 105 | worker_timer2_.stop(); 106 | if (spin_thread_) 107 | { 108 | { 109 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 110 | need_to_terminate_ = true; 111 | } 112 | spin_thread_->join(); 113 | delete spin_thread_; 114 | } 115 | } 116 | 117 | protected: 118 | 119 | /** 120 | * @brief Protected constructor that should be called by subclasses 121 | */ 122 | BaseMerger() : nh_("~msg_merger"), spin_thread_(NULL), need_to_terminate_(false) {} 123 | 124 | /** 125 | * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) 126 | */ 127 | void spinThread() 128 | { 129 | while (nh_.ok()) 130 | { 131 | { 132 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 133 | if (need_to_terminate_) 134 | break; 135 | } 136 | callback_queue_.callAvailable(ros::WallDuration(0.1f)); 137 | } 138 | } 139 | 140 | /** 141 | * @brief The callback of the worker that performs the actual work 142 | */ 143 | void workerCallback(const ros::TimerEvent&) 144 | { 145 | compute(); 146 | } 147 | 148 | /** 149 | * @brief The callback of the worker that performs the actual work 150 | */ 151 | void workerCallback2(const ros::TimerEvent&) 152 | { 153 | 154 | compute2(); 155 | } 156 | 157 | 158 | protected: 159 | ros::Timer worker_timer_; 160 | ros::Timer worker_timer2_; 161 | 162 | 163 | private: 164 | 165 | ros::NodeHandle nh_; 166 | boost::thread* spin_thread_; 167 | ros::CallbackQueue callback_queue_; 168 | boost::mutex terminate_mutex_; 169 | bool need_to_terminate_; 170 | 171 | }; 172 | }; 173 | #endif 174 | -------------------------------------------------------------------------------- /msg_merger/include/msg_merger/msg_merger_plugin.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef MSG_MERGER_PLUGIN_H_ 34 | #define MSG_MERGER_PLUGIN_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace msg_merger_plugin 42 | { 43 | class MergerPlugin : public msg_merger_base::BaseMerger 44 | { 45 | public: 46 | 47 | MergerPlugin(): initialized_(false) 48 | { 49 | } 50 | 51 | void initialize(ros::NodeHandle nh); 52 | 53 | void compute(); 54 | void compute2(); 55 | 56 | void obstSubCB 57 | ( 58 | const teb_local_planner::ObstacleWithTrajectory::ConstPtr& obst_msg, 59 | teb_local_planner::ObstacleWithTrajectory::Ptr& output 60 | ); 61 | 62 | private: 63 | 64 | bool initialized_; //!< Keeps track about the correct initialization of this class 65 | boost::mutex obst_subs_array_mutex_; 66 | ros::NodeHandle nh_; 67 | 68 | std::vector, teb_local_planner::ObstacleWithTrajectory::Ptr>> obst_subs_array_; 69 | 70 | teb_local_planner::ObstacleWithTrajectoryArray obst_array_; 71 | 72 | ros::Publisher obst_array_pub_; 73 | 74 | ros::master::V_TopicInfo topic_infos_; 75 | 76 | }; 77 | } 78 | #endif 79 | -------------------------------------------------------------------------------- /msg_merger/launch/msg_merger.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /msg_merger/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | msg_merger 4 | 0.0.0 5 | The msg_merger package 6 | 7 | 8 | 9 | 10 | user 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | pluginlib 53 | roscpp 54 | teb_local_planner 55 | pluginlib 56 | roscpp 57 | teb_local_planner 58 | roscpp 59 | teb_local_planner 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /msg_merger/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Simple process to copy robot trajectories as obstacle trajectories 4 | 5 | 6 | -------------------------------------------------------------------------------- /msg_merger/src/msg_merger_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | class MergerStandalone 39 | { 40 | public: 41 | MergerStandalone(ros::NodeHandle* nodehandle): n_(*nodehandle), loader_("msg_merger", "msg_merger_base::BaseMerger") 42 | { 43 | 44 | n_ = ros::NodeHandle("~"); 45 | 46 | int rate = 10; 47 | 48 | try 49 | { 50 | merger_ = loader_.createInstance("msg_merger_plugin::MergerPlugin"); 51 | } 52 | catch(const pluginlib::PluginlibException& ex) 53 | { 54 | ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); 55 | ros::shutdown(); 56 | } 57 | 58 | ROS_INFO_STREAM("Standalone msg_merger:" << "msg_merger::MergerPlugin" << " loaded."); 59 | 60 | 61 | if (merger_) 62 | { 63 | merger_->initialize(n_); 64 | merger_->startWorker(ros::Rate(double(rate)), true); 65 | } 66 | 67 | } 68 | 69 | 70 | private: 71 | pluginlib::ClassLoader loader_; 72 | boost::shared_ptr merger_; 73 | 74 | ros::NodeHandle n_; 75 | }; 76 | 77 | int main(int argc, char** argv) 78 | { 79 | ros::init(argc, argv, "msg_merger_node"); 80 | 81 | ros::NodeHandle nh; 82 | 83 | MergerStandalone process(&nh); 84 | 85 | ros::spin(); 86 | return 0; 87 | } -------------------------------------------------------------------------------- /msg_merger/src/msg_merger_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | PLUGINLIB_EXPORT_CLASS(msg_merger_plugin::MergerPlugin, msg_merger_base::BaseMerger) 38 | 39 | namespace msg_merger_plugin 40 | { 41 | 42 | void MergerPlugin::initialize(ros::NodeHandle nh) 43 | { 44 | if(!initialized_) 45 | { 46 | nh_ = nh; 47 | 48 | std::string ns = ""; 49 | { 50 | using namespace ros; 51 | ns = this_node::getNamespace(); 52 | } 53 | 54 | obst_array_pub_ = nh.advertise(ns+"/move_base/TebLocalPlannerROS/dynamic_obstacles",1); 55 | 56 | initialized_ = true; 57 | ROS_DEBUG("msg_merger plugin initialized."); 58 | } 59 | else 60 | { 61 | ROS_WARN("msg_merger plugin has already been initialized, doing nothing."); 62 | } 63 | } 64 | 65 | void MergerPlugin::obstSubCB 66 | ( 67 | const teb_local_planner::ObstacleWithTrajectory::ConstPtr& obst_msg, 68 | teb_local_planner::ObstacleWithTrajectory::Ptr& output 69 | ) 70 | { 71 | boost::mutex::scoped_lock l(obst_subs_array_mutex_); 72 | *output = *obst_msg; 73 | } 74 | 75 | 76 | void MergerPlugin::compute() 77 | { 78 | boost::mutex::scoped_lock l(obst_subs_array_mutex_); 79 | 80 | ros::master::getTopics(topic_infos_); 81 | 82 | for (int i=0; ifirst.get()->getTopic() == topic_infos_.at(i).name) 105 | { 106 | //this subscribed topic no longer has any publisher, unsub and delete; 107 | if (it->first.get()->getNumPublishers()==0) 108 | { 109 | it->first.get()->shutdown(); 110 | it = obst_subs_array_.erase(it); 111 | } 112 | present = true; 113 | break; 114 | } 115 | else 116 | { 117 | it++; 118 | } 119 | } 120 | 121 | // this topic has no subscriber, create one 122 | if (!present) 123 | { 124 | 125 | teb_local_planner::ObstacleWithTrajectory::Ptr msgPtr = boost::make_shared(); 126 | 127 | boost::shared_ptr subPtr = boost::make_shared 128 | ( 129 | nh_.subscribe 130 | ( 131 | topic_infos_.at(i).name, 1, boost::bind(&MergerPlugin::obstSubCB, this, _1, msgPtr) 132 | ) 133 | ); 134 | 135 | 136 | obst_subs_array_.push_back 137 | ( 138 | std::pair, boost::shared_ptr< teb_local_planner::ObstacleWithTrajectory >>(subPtr,msgPtr) 139 | ); 140 | } 141 | 142 | //case of subscribed topics not in the list from master: robot physically removed? 143 | 144 | } 145 | } 146 | } 147 | 148 | void MergerPlugin::compute2() 149 | { 150 | obst_array_.obstacles.clear(); 151 | 152 | boost::mutex::scoped_lock l(obst_subs_array_mutex_); 153 | 154 | 155 | for (int i=0; itrajectory.size()!=0) 159 | { 160 | //remove old obstacles with trajectory 161 | 162 | double last = obst_subs_array_.at(i).second.get()->header.stamp.toSec() + obst_subs_array_.at(i).second.get()->trajectory.end()->time_from_start.toSec(); 163 | if ( /*last 37 | #include 38 | #include 39 | #include 40 | 41 | namespace robot_scan_matcher_base 42 | { 43 | class BaseMatcher 44 | { 45 | public: 46 | virtual void initialize(ros::NodeHandle nh, tf2_ros::Buffer* tf, std::string global_frame, std::string local_frame, 47 | std::string laserScan_topic, double costmap_SizeInMetersX, double costmap_SizeInMetersY, 48 | std::string obst_topic) = 0; 49 | 50 | virtual ~BaseMatcher(){stopWorker();} 51 | 52 | 53 | /** 54 | * @brief This method performs the actual work 55 | */ 56 | virtual void compute() = 0; 57 | 58 | 59 | /** 60 | * @brief Instantiate a worker that repeatedly carry out robot-scan matching. 61 | * The worker is implemented as a timer event that is invoked at a specific \c rate. 62 | * By specifying the argument \c spin_thread the timer event is invoked in a separate 63 | * thread and callback queue or otherwise it is called from the global callback queue (of the 64 | * node in which the plugin is used). 65 | * @param rate The rate that specifies how often the matching should be done. 66 | * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue) 67 | */ 68 | void startWorker(ros::Rate rate, bool spin_thread = false) 69 | { 70 | //setup 71 | 72 | 73 | // 74 | 75 | if (spin_thread_) 76 | { 77 | { 78 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 79 | need_to_terminate_ = true; 80 | } 81 | spin_thread_->join(); 82 | delete spin_thread_; 83 | } 84 | 85 | if (spin_thread) 86 | { 87 | ROS_DEBUG_NAMED("robot_scan_matcher", "Spinning up a thread for the robot_scan_matcher plugin"); 88 | need_to_terminate_ = false; 89 | spin_thread_ = new boost::thread(boost::bind(&BaseMatcher::spinThread, this)); 90 | nh_.setCallbackQueue(&callback_queue_); 91 | } 92 | else 93 | { 94 | spin_thread_ = NULL; 95 | nh_.setCallbackQueue(ros::getGlobalCallbackQueue()); 96 | } 97 | 98 | worker_timer_ = nh_.createTimer(rate, &BaseMatcher::workerCallback, this); 99 | } 100 | 101 | /** 102 | * @brief Stop the worker 103 | */ 104 | void stopWorker() 105 | { 106 | worker_timer_.stop(); 107 | if (spin_thread_) 108 | { 109 | { 110 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 111 | need_to_terminate_ = true; 112 | } 113 | spin_thread_->join(); 114 | delete spin_thread_; 115 | } 116 | } 117 | 118 | protected: 119 | 120 | /** 121 | * @brief Protected constructor that should be called by subclasses 122 | */ 123 | BaseMatcher() : nh_("~robot_scan_matcher"), spin_thread_(NULL), need_to_terminate_(false) {} 124 | 125 | /** 126 | * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) 127 | */ 128 | void spinThread() 129 | { 130 | while (nh_.ok()) 131 | { 132 | { 133 | boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 134 | if (need_to_terminate_) 135 | break; 136 | } 137 | callback_queue_.callAvailable(ros::WallDuration(0.1f)); 138 | } 139 | } 140 | 141 | /** 142 | * @brief The callback of the worker that performs the actual work 143 | */ 144 | void workerCallback(const ros::TimerEvent&) 145 | { 146 | 147 | compute(); 148 | } 149 | 150 | 151 | protected: 152 | ros::Timer worker_timer_; 153 | 154 | 155 | private: 156 | 157 | ros::NodeHandle nh_; 158 | boost::thread* spin_thread_; 159 | ros::CallbackQueue callback_queue_; 160 | boost::mutex terminate_mutex_; 161 | bool need_to_terminate_; 162 | 163 | }; 164 | }; 165 | #endif 166 | -------------------------------------------------------------------------------- /robot_scan_matcher/include/robot_scan_matcher/robot_scan_matcher_plugins.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef ROBOT_SCAN_MATCHER_PLUGINS_H_ 34 | #define ROBOT_SCAN_MATCHER_PLUGINS_H_ 35 | #include 36 | 37 | #include 38 | 39 | // transforms 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | // message types 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | // boost classes 56 | #include 57 | 58 | // dynamic reconfigure 59 | #include 60 | #include 61 | 62 | namespace robot_scan_matcher_plugins 63 | { 64 | class NavieMatcher : public robot_scan_matcher_base::BaseMatcher 65 | { 66 | public: 67 | NavieMatcher() 68 | { 69 | initialized_ = false; 70 | tfListener_ = new tf2_ros::TransformListener(tfBuffer_); 71 | } 72 | 73 | struct Parameters 74 | { 75 | Parameters() : enable_filtering_(true), rate_(10), removal_distance_(0.1), translation_threshold_(0.1) {} 76 | 77 | bool enable_filtering_; //!< enable the matcher to filter laser scan 78 | int rate_; //!< rate of processing 79 | double removal_distance_; //!< distance in meter between scan points and robot model in which removal of points is done 80 | double translation_threshold_; //!< allowed distance in meter for the matching 81 | }; 82 | 83 | /** 84 | * @brief Initializes the matcher plugin 85 | * @param name The name of the instance 86 | * @param tf Pointer to a tf buffer 87 | * @param laserScan_topic laserScan topic (required since TEB planner is not directly exposed to laserscan) 88 | * @param costmap_SizeInMetersX size of the costmap in X, only scan points within this region is considered 89 | * @param costmap_SizeInMetersY size of the costmap in Y, only scan points within this region is considered 90 | * @param obstacles_raw un-altered obstacles array from TEB planner 91 | * @param parent_obst_mutex_ mutex from TEB planner 92 | */ 93 | void initialize(ros::NodeHandle nh, tf2_ros::Buffer* tf, std::string global_frame, std::string local_frame, 94 | std::string laserScan_topic, double costmap_SizeInMetersX, double costmap_SizeInMetersY, 95 | std::string obst_topic); 96 | 97 | void laserScanCB(const sensor_msgs::LaserScan::ConstPtr& laserScan_msg); 98 | 99 | void obstSubCB(const teb_local_planner::ObstacleWithTrajectoryArray::ConstPtr& obst_msg); 100 | 101 | /** 102 | * @brief This method performs the actual work 103 | */ 104 | void compute(); 105 | 106 | teb_local_planner::ObstaclePtr createObstacleFromMsg(teb_local_planner::ObstacleWithTrajectory& curr_obstacle); 107 | 108 | Parameters parameter_; //< active parameters throughout computation 109 | Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure 110 | boost::mutex parameter_mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance 111 | 112 | private: 113 | 114 | void reconfigureCB(robot_scan_matcher::robot_scan_matcherConfig& config, uint32_t level); 115 | 116 | bool initialized_; //!< Keeps track about the correct initialization of this class 117 | 118 | tf2_ros::Buffer* tf_; //!< pointer to tf buffer 119 | std::string global_frame_; 120 | std::string local_frame_; 121 | tf2_ros::Buffer tfBuffer_; 122 | tf2_ros::TransformListener* tfListener_; 123 | 124 | message_filters::Subscriber laserScan_sub_; //!< Subscriber for laserScan since TEB planner is not directly exposed to laserscan 125 | tf2_ros::MessageFilter* laser_filter_; 126 | boost::mutex laserScan_mutex_; 127 | double costmap_SizeInMetersX_; 128 | double costmap_SizeInMetersY_; 129 | 130 | laser_geometry::LaserProjection projector_; 131 | sensor_msgs::PointCloud2 cloud_; 132 | sensor_msgs::PointCloud2 cloud_filtered_; 133 | ros::Publisher cloud_pub_; 134 | ros::Publisher scan_pub_; 135 | 136 | boost::mutex obst_mutex_; //!< mutex for copying obstacles from parent node 137 | ros::Subscriber obst_msg_sub_; 138 | //tf2_ros::MessageFilter* obst_filter_; 139 | teb_local_planner::ObstacleWithTrajectoryArray obstacles_raw_; 140 | teb_local_planner::ObstacleWithTrajectoryArray obstacles_filtered_; 141 | 142 | dynamic_reconfigure::Server* dynamic_recfg_; 143 | 144 | }; 145 | 146 | }; 147 | #endif 148 | -------------------------------------------------------------------------------- /robot_scan_matcher/launch/robot_scan_matcher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /robot_scan_matcher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_scan_matcher 4 | 0.2.0 5 | The robot_scan_matcher package 6 | test 7 | BSD 8 | 9 | 10 | 11 | 12 | 13 | Yiu Ming Chung 14 | 15 | catkin 16 | 17 | pluginlib 18 | roscpp 19 | pcl_ros 20 | teb_local_planner 21 | 22 | pluginlib 23 | roscpp 24 | pcl_ros 25 | teb_local_planner 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /robot_scan_matcher/robot_scan_matcher_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Simple robot-scan matching process to correct costmap 4 | 5 | 6 | -------------------------------------------------------------------------------- /robot_scan_matcher/src/robot_scan_matcher_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | class MatcherStandalone 42 | { 43 | public: 44 | MatcherStandalone(ros::NodeHandle* nodehandle, tf2_ros::Buffer& tf) : 45 | n_(*nodehandle), robot_scan_matcher_loader("robot_scan_matcher", "robot_scan_matcher_base::BaseMatcher"), tf_(tf) 46 | { 47 | 48 | std::string node_ns = ros::this_node::getNamespace(); //this is the robot name in most cases 49 | 50 | n_ = ros::NodeHandle("~"); 51 | 52 | ros::NodeHandle teb_nh(node_ns+"/move_base/TebLocalPlannerROS"); 53 | ros::NodeHandle costmap_nh(node_ns+"/move_base/local_costmap"); 54 | 55 | // load converter plugin from parameter server, otherwise set default 56 | 57 | std::string matcher_plugin = "robot_scan_matcher_plugins::NavieMatcher"; 58 | 59 | std::string global_frame = "robot_odom"; 60 | costmap_nh.param("global_frame", global_frame, global_frame); 61 | std::string robot_base_frame = "robot_base_link"; 62 | costmap_nh.param("robot_base_frame", robot_base_frame, robot_base_frame); 63 | double width = 15; 64 | costmap_nh.param("width", width, width); 65 | double height = 15; 66 | costmap_nh.param("height", height, height); 67 | 68 | std::string scan_topic = "front_laser/scan"; 69 | n_.param("scan_topic", scan_topic, scan_topic); 70 | std::string obstacle_topic = "move_base/TebLocalPlannerROS/dynamic_obstacles"; 71 | n_.param("obstacle_topic", obstacle_topic, obstacle_topic); 72 | 73 | scan_topic = node_ns + "/" + scan_topic; 74 | obstacle_topic = node_ns + "/" + obstacle_topic; 75 | 76 | int rate = 10; 77 | n_.param("rate", rate, rate); 78 | 79 | 80 | try 81 | { 82 | matcher_ = robot_scan_matcher_loader.createInstance(matcher_plugin); 83 | } 84 | catch(const pluginlib::PluginlibException& ex) 85 | { 86 | ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); 87 | ros::shutdown(); 88 | } 89 | 90 | ROS_INFO_STREAM("Standalone robot_scan_matcher:" << matcher_plugin << " loaded."); 91 | 92 | if (matcher_) 93 | { 94 | matcher_->initialize(n_, &tf, global_frame, robot_base_frame, 95 | scan_topic, width, height, 96 | obstacle_topic); 97 | matcher_->startWorker(ros::Rate(double(rate)), true); 98 | } 99 | 100 | } 101 | 102 | private: 103 | pluginlib::ClassLoader robot_scan_matcher_loader; 104 | boost::shared_ptr matcher_; 105 | 106 | ros::NodeHandle n_; 107 | tf2_ros::Buffer& tf_; 108 | 109 | 110 | std::string frame_id_; 111 | 112 | }; 113 | 114 | int main(int argc, char** argv) 115 | { 116 | ros::init(argc, argv, "robot_scan_matcher"); 117 | 118 | ros::NodeHandle nh; 119 | 120 | tf2_ros::Buffer buffer(ros::Duration(5)); 121 | tf2_ros::TransformListener tf(buffer); 122 | 123 | MatcherStandalone match_process(&nh, buffer); 124 | 125 | ros::spin(); 126 | 127 | return 0; 128 | } -------------------------------------------------------------------------------- /robot_scan_matcher/src/robot_scan_matcher_plugins.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BSD 3-Clause License 3 | * 4 | * Copyright (c) 2021, chungym 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * 13 | * 2. Redistributions in binary form must reproduce the above copyright notice, 14 | * this list of conditions and the following disclaimer in the documentation 15 | * and/or other materials provided with the distribution. 16 | * 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived from 19 | * this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | PLUGINLIB_EXPORT_CLASS(robot_scan_matcher_plugins::NavieMatcher, robot_scan_matcher_base::BaseMatcher) 41 | 42 | namespace robot_scan_matcher_plugins 43 | { 44 | 45 | void NavieMatcher::initialize(ros::NodeHandle nh, tf2_ros::Buffer* tf, std::string global_frame, std::string local_frame, 46 | std::string laserScan_topic, double costmap_SizeInMetersX, double costmap_SizeInMetersY, 47 | std::string obst_topic) 48 | { 49 | if(!initialized_) 50 | { 51 | 52 | tf_ = tf; 53 | global_frame_ = global_frame; 54 | local_frame_ = local_frame; 55 | 56 | ROS_INFO_STREAM("LaserScan is transformed to planner frame: "<< global_frame_); 57 | 58 | laserScan_sub_.subscribe(nh, laserScan_topic, 2); 59 | laser_filter_ = new tf2_ros::MessageFilter(laserScan_sub_, tfBuffer_, global_frame_, 5, 0); 60 | laser_filter_->registerCallback( boost::bind(&NavieMatcher::laserScanCB, this, _1) ); 61 | 62 | obst_msg_sub_ = nh.subscribe(obst_topic, 1, &NavieMatcher::obstSubCB, this); 63 | 64 | 65 | costmap_SizeInMetersX_ = costmap_SizeInMetersX; 66 | costmap_SizeInMetersY_ = costmap_SizeInMetersY; 67 | 68 | std::string ns = ""; 69 | { 70 | using namespace ros; 71 | ns = this_node::getNamespace(); 72 | } 73 | 74 | cloud_pub_ = nh.advertise(ns+"/PointCloud_filtered",5); 75 | scan_pub_ = nh.advertise(ns+"/base_scan_filtered",5); 76 | 77 | nh.param("enable_filtering", parameter_.enable_filtering_, true); 78 | nh.param("removal_distance", parameter_.removal_distance_, 0.1); 79 | nh.param("translation_threshold", parameter_.translation_threshold_, 0.1); 80 | 81 | parameter_buffered_ = parameter_; 82 | 83 | // setup dynamic reconfigure 84 | dynamic_recfg_ = new dynamic_reconfigure::Server(nh); 85 | dynamic_reconfigure::Server::CallbackType cb = boost::bind(&NavieMatcher::reconfigureCB, this, _1, _2); 86 | dynamic_recfg_->setCallback(cb); 87 | 88 | initialized_ = true; 89 | ROS_DEBUG("robot_scan_matcher plugin initialized."); 90 | } 91 | else 92 | { 93 | ROS_WARN("robot_scan_matcher plugin has already been initialized, doing nothing."); 94 | } 95 | } 96 | 97 | 98 | void NavieMatcher::laserScanCB(const sensor_msgs::LaserScan::ConstPtr& laserScan_msg) 99 | { 100 | 101 | 102 | boost::mutex::scoped_lock l(laserScan_mutex_); 103 | 104 | try 105 | { 106 | 107 | sensor_msgs::LaserScan temp; 108 | temp = *laserScan_msg; 109 | 110 | // remove any leading slash 111 | if('/' == temp.header.frame_id.front()) 112 | { 113 | temp.header.frame_id.erase(temp.header.frame_id.begin()); 114 | } 115 | 116 | scan_pub_.publish(temp); 117 | 118 | projector_.transformLaserScanToPointCloud(global_frame_, temp, cloud_, tfBuffer_, INFINITY, laser_geometry::channel_option::Intensity); 119 | cloud_.header.stamp = temp.header.stamp; 120 | 121 | } 122 | catch (tf::TransformException& e) 123 | { 124 | std::cout << e.what(); 125 | return; 126 | } 127 | } 128 | 129 | void NavieMatcher::obstSubCB(const teb_local_planner::ObstacleWithTrajectoryArray::ConstPtr& obst_msg) 130 | { 131 | boost::mutex::scoped_lock l(obst_mutex_); 132 | obstacles_raw_ = *obst_msg; 133 | } 134 | 135 | 136 | void NavieMatcher::compute() 137 | { 138 | 139 | { //copy from dynamic reconfigure 140 | boost::mutex::scoped_lock lock(parameter_mutex_); 141 | parameter_ = parameter_buffered_; 142 | } 143 | 144 | pcl::PointCloud pclpc; 145 | 146 | boost::mutex::scoped_lock l(laserScan_mutex_); 147 | // Convert to PCL data type 148 | 149 | 150 | pcl::fromROSMsg(cloud_, pclpc); 151 | 152 | 153 | boost::mutex::scoped_lock l_obst(obst_mutex_); 154 | 155 | 156 | 157 | if (!obstacles_raw_.obstacles.empty()) 158 | { 159 | for (size_t i=0; igetMinimumDistance(Eigen::Vector2d(it->x, it->y)) <= parameter_.removal_distance_) 172 | { 173 | it = pclpc.erase(it); 174 | } 175 | else 176 | { 177 | it++; 178 | } 179 | 180 | } 181 | } 182 | 183 | } 184 | 185 | } 186 | 187 | pcl::toROSMsg(pclpc, cloud_filtered_); 188 | cloud_filtered_.header.stamp = cloud_.header.stamp; 189 | 190 | cloud_pub_.publish(cloud_filtered_); 191 | 192 | } 193 | 194 | teb_local_planner::ObstaclePtr NavieMatcher::createObstacleFromMsg(teb_local_planner::ObstacleWithTrajectory& obstacleMsg) 195 | { 196 | // We only use the global header to specify the obstacle coordinate system instead of individual ones 197 | geometry_msgs::TransformStamped obstacle_to_map; 198 | try 199 | { 200 | obstacle_to_map = tf_->lookupTransform(global_frame_, ros::Time(0), 201 | obstacleMsg.header.frame_id, ros::Time(0), 202 | obstacleMsg.header.frame_id, ros::Duration(0.1)); 203 | } 204 | catch (tf::TransformException ex) 205 | { 206 | ROS_ERROR("%s",ex.what()); 207 | } 208 | 209 | double theta_offset = tf::getYaw(obstacle_to_map.transform.rotation); 210 | 211 | //time passed since the creation of message 212 | ros::Duration time_offset = ros::Time::now() - obstacleMsg.header.stamp; 213 | 214 | //make a copy from the original message, otherwise will be recursely offset in incorrect way 215 | std::vector> trajectory_copy (obstacleMsg.trajectory); 216 | 217 | //compute relative time from now, and transform pose 218 | for (unsigned int j = 0; j < trajectory_copy.size(); j++) 219 | { 220 | trajectory_copy.at(j).time_from_start -= time_offset; 221 | 222 | // T = T2*T1 = (R2 t2) (R1 t1) = (R2*R1 R2*t1+t2) 223 | // (0 1) (0 1) (0 1) 224 | trajectory_copy.at(j).pose.x = obstacleMsg.trajectory.at(j).pose.x*cos(theta_offset) - obstacleMsg.trajectory.at(j).pose.y*sin(theta_offset) 225 | + obstacle_to_map.transform.translation.x; 226 | trajectory_copy.at(j).pose.y = obstacleMsg.trajectory.at(j).pose.x*sin(theta_offset) + obstacleMsg.trajectory.at(j).pose.y*cos(theta_offset) 227 | + obstacle_to_map.transform.translation.y; 228 | trajectory_copy.at(j).pose.theta = obstacleMsg.trajectory.at(j).pose.theta + theta_offset; 229 | } 230 | 231 | teb_local_planner::ObstaclePtr obst_ptr(NULL); 232 | if (obstacleMsg.footprint.type == teb_local_planner::ObstacleFootprint::CircularObstacle) // circle 233 | { 234 | teb_local_planner::PoseSE2 pose_init; 235 | Eigen::Vector2d speed_init; 236 | teb_local_planner::CircularObstacle* obstacle = new teb_local_planner::CircularObstacle; 237 | obstacle->setTrajectory(trajectory_copy, obstacleMsg.footprint.holonomic); 238 | obstacle->predictPoseFromTrajectory(0, pose_init, speed_init); 239 | obstacle->x() = pose_init.x(); 240 | obstacle->y() = pose_init.y(); 241 | obstacle->radius() = obstacleMsg.footprint.radius; 242 | obstacle->setCentroidVelocity(speed_init); 243 | obst_ptr = teb_local_planner::ObstaclePtr(obstacle); 244 | } 245 | else if (obstacleMsg.footprint.type == teb_local_planner::ObstacleFootprint::PointObstacle ) // point 246 | { 247 | teb_local_planner::PoseSE2 pose_init; 248 | Eigen::Vector2d speed_init; 249 | teb_local_planner::PointObstacle* obstacle = new teb_local_planner::PointObstacle; 250 | obstacle->setTrajectory(trajectory_copy, obstacleMsg.footprint.holonomic); 251 | obstacle->predictPoseFromTrajectory(0, pose_init, speed_init); 252 | obstacle->x() = pose_init.x(); 253 | obstacle->y() = pose_init.y(); 254 | obstacle->setCentroidVelocity(speed_init); 255 | obst_ptr = teb_local_planner::ObstaclePtr(obstacle); 256 | } 257 | else if (obstacleMsg.footprint.type == teb_local_planner::ObstacleFootprint::LineObstacle ) // line 258 | { 259 | Eigen::Vector2d start_robot( obstacleMsg.footprint.point1.x, 260 | obstacleMsg.footprint.point1.y); 261 | Eigen::Vector2d end_robot( obstacleMsg.footprint.point2.x, 262 | obstacleMsg.footprint.point2.y); 263 | teb_local_planner::PoseSE2 pose_init; 264 | Eigen::Vector2d speed_init; 265 | teb_local_planner::LineObstacle* obstacle = new teb_local_planner::LineObstacle; 266 | Eigen::Vector2d start; 267 | Eigen::Vector2d end; 268 | obstacle->setTrajectory(trajectory_copy, obstacleMsg.footprint.holonomic); 269 | obstacle->predictPoseFromTrajectory(0, pose_init, speed_init); 270 | obstacle->transformToWorld(pose_init, start_robot, end_robot, start, end); 271 | obstacle->setStart(start); 272 | obstacle->setEnd(end); 273 | obstacle->setCentroidVelocity(speed_init); 274 | obst_ptr = teb_local_planner::ObstaclePtr(obstacle); 275 | } 276 | else if (obstacleMsg.footprint.type == teb_local_planner::ObstacleFootprint::PillObstacle ) // pill 277 | { 278 | Eigen::Vector2d start_robot( obstacleMsg.footprint.point1.x, 279 | obstacleMsg.footprint.point1.y); 280 | Eigen::Vector2d end_robot( obstacleMsg.footprint.point2.x, 281 | obstacleMsg.footprint.point2.y); 282 | teb_local_planner::PoseSE2 pose_init; 283 | Eigen::Vector2d speed_init; 284 | teb_local_planner::PillObstacle* obstacle = new teb_local_planner::PillObstacle; 285 | Eigen::Vector2d start; 286 | Eigen::Vector2d end; 287 | obstacle->setTrajectory(trajectory_copy, obstacleMsg.footprint.holonomic); 288 | obstacle->predictPoseFromTrajectory(0, pose_init, speed_init); 289 | obstacle->transformToWorld(pose_init, start_robot, end_robot, start, end); 290 | obstacle->setStart(start); 291 | obstacle->setEnd(end); 292 | obstacle->setRadius(obstacleMsg.footprint.radius); 293 | obstacle->setCentroidVelocity(speed_init); 294 | obst_ptr = teb_local_planner::ObstaclePtr(obstacle); 295 | } 296 | else if(obstacleMsg.footprint.type == teb_local_planner::ObstacleFootprint::PolygonObstacle ) // polygon 297 | { 298 | teb_local_planner::Point2dContainer vertices_robocentric; 299 | for (size_t j=0; jsetTrajectory(trajectory_copy, obstacleMsg.footprint.holonomic); 311 | polyobst->predictPoseFromTrajectory(0, pose_init, speed_init); 312 | polyobst->setCentroidVelocity(speed_init); 313 | polyobst->transformToWorld(pose_init, vertices_robocentric, vertices); 314 | 315 | for (size_t j=0; jpushBackVertex( vertices.at(j) ); 318 | } 319 | polyobst->finalizePolygon(); 320 | obst_ptr = teb_local_planner::ObstaclePtr(polyobst); 321 | } 322 | else 323 | { 324 | ROS_WARN("Invalid custom obstacle received. Invalid Type. Skipping..."); 325 | } 326 | 327 | return obst_ptr; 328 | 329 | } 330 | 331 | 332 | void NavieMatcher::reconfigureCB(robot_scan_matcher::robot_scan_matcherConfig& config, uint32_t level) 333 | { 334 | boost::mutex::scoped_lock lock(parameter_mutex_); 335 | parameter_buffered_.enable_filtering_ = config.enable_filtering; 336 | parameter_buffered_.removal_distance_ = config.removal_distance; 337 | parameter_buffered_.translation_threshold_ = config.removal_distance; 338 | parameter_buffered_.rate_ = config.rate; 339 | 340 | if (parameter_buffered_.rate_ != parameter_.rate_){worker_timer_.setPeriod(ros::Duration(1.0/parameter_buffered_.rate_), false);} 341 | } 342 | 343 | } --------------------------------------------------------------------------------