From 49b21679835d2b50707fa9f5d13c248f596d1881 Mon Sep 17 00:00:00 2001 From: Brian Date: Fri, 14 Sep 2018 16:12:28 -0700 Subject: [PATCH 1/2] adding costmap_2d from ROS1 navstack --- .../nav2_util/src/costmap_2d/CHANGELOG.rst | 203 +++ .../nav2_util/src/costmap_2d/CMakeLists.txt | 206 +++ .../src/costmap_2d/cfg/Costmap2D.cfg | 23 + .../src/costmap_2d/cfg/GenericPlugin.cfg | 7 + .../src/costmap_2d/cfg/InflationPlugin.cfg | 12 + .../src/costmap_2d/cfg/ObstaclePlugin.cfg | 20 + .../src/costmap_2d/cfg/VoxelPlugin.cfg | 22 + .../src/costmap_2d/costmap_plugins.xml | 17 + .../include/costmap_2d/array_parser.h | 51 + .../include/costmap_2d/cost_values.h | 47 + .../include/costmap_2d/costmap_2d.h | 469 +++++++ .../include/costmap_2d/costmap_2d_publisher.h | 109 ++ .../include/costmap_2d/costmap_2d_ros.h | 271 ++++ .../include/costmap_2d/costmap_layer.h | 149 +++ .../include/costmap_2d/costmap_math.h | 69 + .../costmap_2d/include/costmap_2d/footprint.h | 147 ++ .../include/costmap_2d/inflation_layer.h | 198 +++ .../src/costmap_2d/include/costmap_2d/layer.h | 134 ++ .../include/costmap_2d/layered_costmap.h | 177 +++ .../include/costmap_2d/observation.h | 103 ++ .../include/costmap_2d/observation_buffer.h | 164 +++ .../include/costmap_2d/obstacle_layer.h | 177 +++ .../include/costmap_2d/static_layer.h | 101 ++ .../include/costmap_2d/testing_helper.h | 102 ++ .../include/costmap_2d/voxel_layer.h | 151 +++ .../src/costmap_2d/launch/example.launch | 21 + .../src/costmap_2d/launch/example_params.yaml | 40 + .../src/costmap_2d/msg/VoxelGrid.msg | 8 + src/libs/nav2_util/src/costmap_2d/package.xml | 59 + .../costmap_2d/plugins/inflation_layer.cpp | 383 ++++++ .../src/costmap_2d/plugins/obstacle_layer.cpp | 620 +++++++++ .../src/costmap_2d/plugins/static_layer.cpp | 346 +++++ .../src/costmap_2d/plugins/voxel_layer.cpp | 442 +++++++ .../src/costmap_2d/src/array_parser.cpp | 115 ++ .../src/costmap_2d/src/costmap_2d.cpp | 484 +++++++ .../src/costmap_2d/src/costmap_2d_cloud.cpp | 205 +++ .../src/costmap_2d/src/costmap_2d_markers.cpp | 153 +++ .../src/costmap_2d/src/costmap_2d_node.cpp | 50 + .../costmap_2d/src/costmap_2d_publisher.cpp | 169 +++ .../src/costmap_2d/src/costmap_2d_ros.cpp | 572 ++++++++ .../src/costmap_2d/src/costmap_layer.cpp | 142 ++ .../src/costmap_2d/src/costmap_math.cpp | 89 ++ .../src/costmap_2d/src/footprint.cpp | 325 +++++ .../nav2_util/src/costmap_2d/src/layer.cpp | 56 + .../src/costmap_2d/src/layered_costmap.cpp | 185 +++ .../src/costmap_2d/src/observation_buffer.cpp | 258 ++++ .../src/costmap_2d/test/TenByTen.pgm | Bin 0 -> 152 bytes .../src/costmap_2d/test/TenByTen.yaml | 7 + .../src/costmap_2d/test/array_parser_test.cpp | 80 ++ .../src/costmap_2d/test/costmap_params.yaml | 29 + .../src/costmap_2d/test/costmap_tester.cpp | 145 ++ .../src/costmap_2d/test/footprint_tests.cpp | 178 +++ .../costmap_2d/test/footprint_tests.launch | 29 + .../src/costmap_2d/test/inflation_tests.cpp | 411 ++++++ .../costmap_2d/test/inflation_tests.launch | 6 + .../src/costmap_2d/test/module_tests.cpp | 1177 +++++++++++++++++ .../src/costmap_2d/test/obstacle_tests.cpp | 252 ++++ .../src/costmap_2d/test/obstacle_tests.launch | 5 + .../costmap_2d/test/simple_driving_test.xml | 14 + .../src/costmap_2d/test/static_tests.cpp | 331 +++++ .../src/costmap_2d/test/static_tests.launch | 5 + 61 files changed, 10520 insertions(+) create mode 100644 src/libs/nav2_util/src/costmap_2d/CHANGELOG.rst create mode 100644 src/libs/nav2_util/src/costmap_2d/CMakeLists.txt create mode 100755 src/libs/nav2_util/src/costmap_2d/cfg/Costmap2D.cfg create mode 100755 src/libs/nav2_util/src/costmap_2d/cfg/GenericPlugin.cfg create mode 100755 src/libs/nav2_util/src/costmap_2d/cfg/InflationPlugin.cfg create mode 100755 src/libs/nav2_util/src/costmap_2d/cfg/ObstaclePlugin.cfg create mode 100755 src/libs/nav2_util/src/costmap_2d/cfg/VoxelPlugin.cfg create mode 100644 src/libs/nav2_util/src/costmap_2d/costmap_plugins.xml create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/array_parser.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/cost_values.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_publisher.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_ros.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_layer.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_math.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/footprint.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/inflation_layer.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layer.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layered_costmap.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation_buffer.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/obstacle_layer.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/static_layer.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/testing_helper.h create mode 100644 src/libs/nav2_util/src/costmap_2d/include/costmap_2d/voxel_layer.h create mode 100644 src/libs/nav2_util/src/costmap_2d/launch/example.launch create mode 100644 src/libs/nav2_util/src/costmap_2d/launch/example_params.yaml create mode 100644 src/libs/nav2_util/src/costmap_2d/msg/VoxelGrid.msg create mode 100644 src/libs/nav2_util/src/costmap_2d/package.xml create mode 100644 src/libs/nav2_util/src/costmap_2d/plugins/inflation_layer.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/plugins/obstacle_layer.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/plugins/static_layer.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/plugins/voxel_layer.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/array_parser.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_2d.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_2d_cloud.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_2d_markers.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_2d_node.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_2d_publisher.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_2d_ros.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_layer.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/costmap_math.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/footprint.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/layer.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/layered_costmap.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/src/observation_buffer.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/TenByTen.pgm create mode 100644 src/libs/nav2_util/src/costmap_2d/test/TenByTen.yaml create mode 100644 src/libs/nav2_util/src/costmap_2d/test/array_parser_test.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/costmap_params.yaml create mode 100644 src/libs/nav2_util/src/costmap_2d/test/costmap_tester.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/footprint_tests.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/footprint_tests.launch create mode 100644 src/libs/nav2_util/src/costmap_2d/test/inflation_tests.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/inflation_tests.launch create mode 100644 src/libs/nav2_util/src/costmap_2d/test/module_tests.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.launch create mode 100644 src/libs/nav2_util/src/costmap_2d/test/simple_driving_test.xml create mode 100644 src/libs/nav2_util/src/costmap_2d/test/static_tests.cpp create mode 100644 src/libs/nav2_util/src/costmap_2d/test/static_tests.launch diff --git a/src/libs/nav2_util/src/costmap_2d/CHANGELOG.rst b/src/libs/nav2_util/src/costmap_2d/CHANGELOG.rst new file mode 100644 index 0000000000..3316d09785 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/CHANGELOG.rst @@ -0,0 +1,203 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package costmap_2d +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.15.2 (2018-03-22) +------------------- +* Merge pull request `#673 `_ from ros-planning/email_update_lunar + update maintainer email (lunar) +* Merge pull request `#670 `_ from DLu/fix206_lunar + Fixes `#206 `_ for Lunar +* fix 'enable' for static_layer with rolling window (`#659 `_) (`#665 `_) +* Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy + Add myself as a maintainer. +* Contributors: Aaron Hoy, David V. Lu!!, Jannik Abbenseth, Michael Ferguson + +1.15.1 (2017-08-14) +------------------- + +1.15.0 (2017-08-07) +------------------- +* Added parameter for allowing inflation in unknown cells (`#564 `_) +* Inflation Layer protected members and virtual computeCost [ABI BREAKING] +* Fix for `#517 `_: create a getRobotPose method on move_base instead of using that on the costmaps +* don't update costs if inflation radius is zero +* rebase fixups +* convert packages to format2 +* Speedup (~60%) inflation layer update (`#525 `_) +* Fix CMakeLists + package.xmls (`#548 `_) +* add missing deps on libpcl +* import only PCL common +* pcl proagate -lQt5::Widgets flag so we need to find_package Qt5Widgets (`#578 `_) +* Added deps to amcl costmap_2d move_base (`#512 `_) +* remove GCC warnings +* Fix CMake warnings +* renamed targets for message generation (gencpp -> generate_messages_cpp) in order to avoid warnings for non-existing target dependencies +* Fixed race condition with costmaps +* Merge pull request `#491 `_ from alexhenning/kinetic-inflation-fix +* Fixed sign error in inflation layer +* Adds warning when a layer shrinks the bounds +* Fixed bug with inflation layer that caused underinflation +* Fixed bug with artifacts when not current +* Fix bug with inflation artifacts being left behind +* Fixes issue with costmaps shearing +* Made costmap publishing truly lazy +* Contributors: Alex Henning, Alexander Reimann, Hidde Wieringa, Jorge Santos, Jorge Santos Simón, Martin Günther, Michael Ferguson, Mikael Arguedas, Stephan Opfer, Vincent Rabaud, mryellow + +1.14.0 (2016-05-20) +------------------- +* Reordered initializer list to match order of declarations. + This avoids compiler warning with some compilers. +* Made update map threadsafe + This is necessary for some plugins (e.g. VoxelLayer) that implement a + thread unsafe updateBounds() function. +* Fix bug with resetting static layer + If we don't have a new topic, consider our old data as if it were new. +* fix resource locations to fix tests +* Increase time-limit on failing test +* Merge pull request `#388 `_ from yujinrobot/jade_inflation_ghost_fix + No more ghosts in the inflation layer +* Fixes the dynamic reconfigure segfault + Doing a dynamic reconfigure of the inflation radius recreates + the cached cost values without first locking a mutex, which causes + a segfault. This breaks the reconfigure of inflation parameters into + a separate function and adds a mutex lock. +* Merge pull request `#415 `_ from alexhenning/jade-fix-multiple-static-layers + Fixes an issue with having multiple static layers +* Fixes an issue with having multiple static layers + If you have a static layer in both the local and global costmaps that + use the same map topic, there is a race condition that can cause the + static layer to get stuck after printing `Requesting map....`. This race + condition seems to be due to the call to shutdown in deactivate and how + the NodeHandle handles multiple subscribers under the hood. + This issue appears to happen about 1 in 1000 times in the setup I was + testing. This fix has never failed in over 1000000 tests. Instead of + calling activate and deactivate, the publisher is only recreated if the + topic has changed. Otherwise, it reuses the old setup. +* fix related to issue `#408 `_ - With Rolling Window on, costmap_2d not properly updating bounds and costs in the static layer +* No more ghosts in the inflation layer + Previous bounds would fit the sensor measurements, and the inflation layer would clear + out to these, but leave 'ghosts' behind. These ghosts are from two sources - 1) the + inflation radius and 2) whole obstacles left behind as the robot has moved from the last point. + The modifications here remember the last bounds and set the new bounds so that a box at least + large enough to incorporate the old bounds plus the inflation radius is generated. +* Contributors: Alex Henning, Daniel Stonier, Levon Avagyan, Michael Ferguson, palmieri + +1.13.1 (2015-10-29) +------------------- +* Remove excessive canTransform spam. +* Fix for `#382 `_ +* Republish costmap if origin changes +* Remove Footprint Layer +* Remove extra sign definition and use proper one when padding footprint +* fix plugin warnings on throw, closes `#205 `_ +* initialize publisher variables +* Look for robot_radius when footprint is not set. `#206 `_ +* Add a first_map_only parameter so we keep reusing the first received static map +* Merge pull request `#331 `_ from mikeferguson/static_layer_any_frame +* support rolling static map in any frame +* fix destructor of Costmap2D +* proper locking during costmap update +* do not resize static map when rolling +* Static layer works with rolling window now +* Contributors: Daniel Stonier, David Lu, Jihoon Lee, Michael Ferguson, Rein Appeldoorn, commaster90 + +1.13.0 (2015-03-17) +------------------- +* fixed issue with voxel_layer and obstacle_layer both deleting the same dynamic_reconfigure::Server and causing segfaults +* Fixing various memory freeing operations +* static_layer: Fix indexing error in OccupancyGridUpdate callback function. +* Contributors: Alex Bencz, David V. Lu!!, James Servos, Julse, Kaijen Hsiao + +1.12.0 (2015-02-04) +------------------- +* update maintainer email +* Contributors: Michael Ferguson + +1.11.15 (2015-02-03) +-------------------- +* Add ARCHIVE_DESTINATION for static builds +* Contributors: Gary Servin + +1.11.14 (2014-12-05) +-------------------- +* added waitForTransform to bufferCloud to solve extrapolation into the future exception +* deallocate costmap_ before reallocating +* prevent div by zero in raytraceLine +* only prefix sensor_frame when it's not empty +* tf_prefix support in obstacle_layer +* remove undefined function updateUsingPlugins +* remove unused cell_data.h +* numerous style fixes +* Contributors: Andrzej Pronobis, David Lu, Jeremie Deray, Mani Monajjemi, Michael Ferguson, enriquefernandez + +1.11.13 (2014-10-02) +-------------------- + +1.11.12 (2014-10-01) +-------------------- +* costmap_2d: export library layers +* Merge pull request `#198 `_ from kmhallen/hydro-devel + Fixed costmap_2d clearing from service /move_base/clear_costmaps +* Costmap Layer comments +* Add destructors for all of the layers to remove the dynamic parameter clients +* Add method for removing static observations (for testing) +* Move testing_helper +* Initial Clearing Costmap parameter change +* Fixed costmap_2d clearing from service /move_base/clear_costmaps +* Contributors: David Lu!!, Kevin Hallenbeck, Michael Ferguson + +1.11.11 (2014-07-23) +-------------------- +* removes trailing spaces and empty lines +* Contributors: Enrique Fernández Perdomo + +1.11.10 (2014-06-25) +-------------------- +* Remove unnecessary colons +* Remove unused robot_radius parameter from dynamic_reconfigure +* Contributors: Daniel Stonier, David Lu!! + +1.11.9 (2014-06-10) +------------------- +* fix hypot issues, add comments to tests from tracking this down +* dynamically reconfigure the previously uninitialised variable 'combination_method', closes `#187 `_. +* uses ::hypot(x, y) instead of sqrt(x*x, y*y) +* Contributors: Daniel Stonier, Michael Ferguson, Enrique Fernández Perdomo + +1.11.8 (2014-05-21) +------------------- + +1.11.7 (2014-05-21) +------------------- +* uses %u instead of %d for unsigned int +* update build to find eigen using cmake_modules +* inflation_layer: place .top() & .pop() calls together +* add parameter to configure whether full costmap is published each time +* Contributors: Michael Ferguson, Siegfried-A. Gevatter Pujals, agentx3r, enriquefernandez + +1.11.5 (2014-01-30) +------------------- +* Better threading in inflation layer +* don't set initialized until updateMap is called +* check whether costmap is initalized before publishing +* New Overwrite Methods + updateMap method + Fix for `#68 `_ + Fix for inflation memory problems + InfIsValid `#128 `_ + Static layer can recieve updates and accept non-lethal values + Obstacle layer uses track_unknown_space parameter + Footprint layer is not longer created as top-level layer (used as part of obstacle layer instead) +* Download test data from download.ros.org instead of willow +* Change maintainer from Hersh to Lu + +1.11.4 (2013-09-27) +------------------- +* Improve bounds checking +* Reimplement Clear Costmaps Service by implementing reset functions in each layer +* Package URL Updates +* Additional static layer functionality for receiving updates +* Misc. Pointcloud fixes +* Improved eigen alignment problem on 32-bit arch. +* fixed costmap_2d tests diff --git a/src/libs/nav2_util/src/costmap_2d/CMakeLists.txt b/src/libs/nav2_util/src/costmap_2d/CMakeLists.txt new file mode 100644 index 0000000000..df0d081817 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 2.8.3) +project(costmap_2d) + +find_package(catkin REQUIRED + COMPONENTS + cmake_modules + dynamic_reconfigure + geometry_msgs + laser_geometry + map_msgs + message_filters + message_generation + nav_msgs + pcl_conversions + pcl_ros + pluginlib + roscpp + sensor_msgs + std_msgs + tf + visualization_msgs + voxel_grid + ) + +find_package(PCL REQUIRED COMPONENTS common) +remove_definitions(-DDISABLE_LIBUSB-1.0) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread) +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +add_definitions(${EIGEN3_DEFINITIONS}) + +# messages +add_message_files( + DIRECTORY msg + FILES + VoxelGrid.msg +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + map_msgs +) + +# dynamic reconfigure +generate_dynamic_reconfigure_options( + cfg/Costmap2D.cfg + cfg/ObstaclePlugin.cfg + cfg/GenericPlugin.cfg + cfg/InflationPlugin.cfg + cfg/VoxelPlugin.cfg +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES costmap_2d layers + CATKIN_DEPENDS + dynamic_reconfigure + geometry_msgs + laser_geometry + map_msgs + message_filters + message_runtime + nav_msgs + pcl_ros + pluginlib + roscpp + sensor_msgs + std_msgs + tf + visualization_msgs + voxel_grid + DEPENDS + PCL + EIGEN3 + Boost +) + +add_library(costmap_2d + src/array_parser.cpp + src/costmap_2d.cpp + src/observation_buffer.cpp + src/layer.cpp + src/layered_costmap.cpp + src/costmap_2d_ros.cpp + src/costmap_2d_publisher.cpp + src/costmap_math.cpp + src/footprint.cpp + src/costmap_layer.cpp +) +add_dependencies(costmap_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(costmap_2d + ${PCL_LIBRARIES} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +add_library(layers + plugins/inflation_layer.cpp + plugins/obstacle_layer.cpp + plugins/static_layer.cpp + plugins/voxel_layer.cpp + src/observation_buffer.cpp +) +add_dependencies(layers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(layers + costmap_2d +) + +add_executable(costmap_2d_markers src/costmap_2d_markers.cpp) +add_dependencies(costmap_2d_markers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(costmap_2d_markers + costmap_2d + ) + +add_executable(costmap_2d_cloud src/costmap_2d_cloud.cpp) +add_dependencies(costmap_2d_cloud ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(costmap_2d_cloud + costmap_2d + ) + +add_executable(costmap_2d_node src/costmap_2d_node.cpp) +add_dependencies(costmap_2d_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(costmap_2d_node + costmap_2d + ) + +## Configure Tests +if(CATKIN_ENABLE_TESTING) + # Find package test dependencies + find_package(rostest REQUIRED) + + # Add the test folder to the include directories + include_directories(test) + + # Create targets for test executables + add_executable(costmap_tester EXCLUDE_FROM_ALL test/costmap_tester.cpp) + add_dependencies(tests costmap_tester) + target_link_libraries(costmap_tester costmap_2d ${GTEST_LIBRARIES}) + + add_executable(footprint_tests EXCLUDE_FROM_ALL test/footprint_tests.cpp) + add_dependencies(tests footprint_tests) + target_link_libraries(footprint_tests costmap_2d ${GTEST_LIBRARIES}) + + add_executable(obstacle_tests EXCLUDE_FROM_ALL test/obstacle_tests.cpp) + add_dependencies(tests obstacle_tests) + target_link_libraries(obstacle_tests costmap_2d layers ${GTEST_LIBRARIES}) + + add_executable(static_tests EXCLUDE_FROM_ALL test/static_tests.cpp) + add_dependencies(tests static_tests) + target_link_libraries(static_tests costmap_2d layers ${GTEST_LIBRARIES}) + + add_executable(inflation_tests EXCLUDE_FROM_ALL test/inflation_tests.cpp) + add_dependencies(tests inflation_tests) + target_link_libraries(inflation_tests costmap_2d layers ${GTEST_LIBRARIES}) + + catkin_download_test_data(${PROJECT_NAME}_simple_driving_test_indexed.bag + http://download.ros.org/data/costmap_2d/simple_driving_test_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 61168cff9425b11e093ea3a627c81c8d) + catkin_download_test_data(${PROJECT_NAME}_willow-full-0.025.pgm + http://download.ros.org/data/costmap_2d/willow-full-0.025.pgm + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 e66b17ee374f2d7657972efcb3e2e4f7) + + add_rostest(test/footprint_tests.launch) + add_rostest(test/inflation_tests.launch) + add_rostest(test/obstacle_tests.launch) + add_rostest(test/simple_driving_test.xml) + add_rostest(test/static_tests.launch) + + catkin_add_gtest(array_parser_test test/array_parser_test.cpp) + target_link_libraries(array_parser_test costmap_2d) +endif() + +install( TARGETS + costmap_2d_markers + costmap_2d_cloud + costmap_2d_node + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS + costmap_2d + layers + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +install(FILES costmap_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + PATTERN ".svn" EXCLUDE +) diff --git a/src/libs/nav2_util/src/costmap_2d/cfg/Costmap2D.cfg b/src/libs/nav2_util/src/costmap_2d/cfg/Costmap2D.cfg new file mode 100755 index 0000000000..772eb1cd98 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/cfg/Costmap2D.cfg @@ -0,0 +1,23 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t + +gen = ParameterGenerator() + +gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) +gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) +gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100) + +#map params +gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) +gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) +gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) +gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) +gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) + +# robot footprint shape +gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") +gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) +gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) + +exit(gen.generate("costmap_2d", "costmap_2d", "Costmap2D")) diff --git a/src/libs/nav2_util/src/costmap_2d/cfg/GenericPlugin.cfg b/src/libs/nav2_util/src/costmap_2d/cfg/GenericPlugin.cfg new file mode 100755 index 0000000000..11cd4d14e0 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/cfg/GenericPlugin.cfg @@ -0,0 +1,7 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t + +gen = ParameterGenerator() +gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) +exit(gen.generate("costmap_2d", "costmap_2d", "GenericPlugin")) diff --git a/src/libs/nav2_util/src/costmap_2d/cfg/InflationPlugin.cfg b/src/libs/nav2_util/src/costmap_2d/cfg/InflationPlugin.cfg new file mode 100755 index 0000000000..c15152581f --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/cfg/InflationPlugin.cfg @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t + +gen = ParameterGenerator() + +gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) +gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) +gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) +gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) + +exit(gen.generate("costmap_2d", "costmap_2d", "InflationPlugin")) diff --git a/src/libs/nav2_util/src/costmap_2d/cfg/ObstaclePlugin.cfg b/src/libs/nav2_util/src/costmap_2d/cfg/ObstaclePlugin.cfg new file mode 100755 index 0000000000..7b434a8e78 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/cfg/ObstaclePlugin.cfg @@ -0,0 +1,20 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t + +gen = ParameterGenerator() + +gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) +gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) +gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) + +combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), + gen.const("Maximum", int_t, 1, "Take the maximum of the values"), + gen.const("Nothing", int_t, 99, "Do nothing")], + "Method for combining layers enum") +gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) + + +#gen.add("max_obstacle_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) +#gen.add("raytrace_range", double_t, 0, "The default range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) +exit(gen.generate("costmap_2d", "costmap_2d", "ObstaclePlugin")) diff --git a/src/libs/nav2_util/src/costmap_2d/cfg/VoxelPlugin.cfg b/src/libs/nav2_util/src/costmap_2d/cfg/VoxelPlugin.cfg new file mode 100755 index 0000000000..ec621c5ae2 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/cfg/VoxelPlugin.cfg @@ -0,0 +1,22 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t + +gen = ParameterGenerator() + +gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) +gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) +gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) +gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) +gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) +gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) +gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) +gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) + +combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), + gen.const("Maximum", int_t, 1, "Take the maximum of the values"), + gen.const("Nothing", int_t, 99, "Do nothing")], + "Method for combining layers enum") +gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) + +exit(gen.generate("costmap_2d", "costmap_2d", "VoxelPlugin")) diff --git a/src/libs/nav2_util/src/costmap_2d/costmap_plugins.xml b/src/libs/nav2_util/src/costmap_2d/costmap_plugins.xml new file mode 100644 index 0000000000..800f0288de --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/costmap_plugins.xml @@ -0,0 +1,17 @@ + + + + Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles. + + + Listens to laser scan and point cloud messages and marks and clears grid cells. + + + Listens to OccupancyGrid messages and copies them in, like from map_server. + + + Similar to obstacle costmap, but uses 3D voxel grid to store data. + + + + diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/array_parser.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/array_parser.h new file mode 100644 index 0000000000..0275bde2f6 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/array_parser.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * author: Dave Hershberger + */ +#ifndef COSTMAP_2D_ARRAY_PARSER_H +#define COSTMAP_2D_ARRAY_PARSER_H + +#include +#include + +namespace costmap_2d +{ + +/** @brief Parse a vector of vectors of floats from a string. + * @param error_return If no error, error_return is set to "". If + * error, error_return will describe the error. + * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] + * + * On error, error_return is set and the return value could be + * anything, like part of a successful parse. */ +std::vector > parseVVF(const std::string& input, std::string& error_return); + +} // end namespace costmap_2d + +#endif // COSTMAP_2D_ARRAY_PARSER_H diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/cost_values.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/cost_values.h new file mode 100644 index 0000000000..7f042ff48b --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/cost_values.h @@ -0,0 +1,47 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#ifndef COSTMAP_2D_COST_VALUES_H_ +#define COSTMAP_2D_COST_VALUES_H_ +/** Provides a mapping for often used cost values */ +namespace costmap_2d +{ +static const unsigned char NO_INFORMATION = 255; +static const unsigned char LETHAL_OBSTACLE = 254; +static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; +static const unsigned char FREE_SPACE = 0; +} +#endif // COSTMAP_2D_COST_VALUES_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d.h new file mode 100644 index 0000000000..38658e840d --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d.h @@ -0,0 +1,469 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_COSTMAP_2D_H_ +#define COSTMAP_2D_COSTMAP_2D_H_ + +#include +#include +#include +#include + +namespace costmap_2d +{ + +// convenient for storing x/y point pairs +struct MapLocation +{ + unsigned int x; + unsigned int y; +}; + +/** + * @class Costmap2D + * @brief A 2D costmap provides a mapping between points in the world and their associated "costs". + */ +class Costmap2D +{ + friend class CostmapTester; // Need this for gtest to work correctly +public: + /** + * @brief Constructor for a costmap + * @param cells_size_x The x size of the map in cells + * @param cells_size_y The y size of the map in cells + * @param resolution The resolution of the map in meters/cell + * @param origin_x The x origin of the map + * @param origin_y The y origin of the map + * @param default_value Default Value + */ + Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, + double origin_x, double origin_y, unsigned char default_value = 0); + + /** + * @brief Copy constructor for a costmap, creates a copy efficiently + * @param map The costmap to copy + */ + Costmap2D(const Costmap2D& map); + + /** + * @brief Overloaded assignment operator + * @param map The costmap to copy + * @return A reference to the map after the copy has finished + */ + Costmap2D& operator=(const Costmap2D& map); + + /** + * @brief Turn this costmap into a copy of a window of a costmap passed in + * @param map The costmap to copy + * @param win_origin_x The x origin (lower left corner) for the window to copy, in meters + * @param win_origin_y The y origin (lower left corner) for the window to copy, in meters + * @param win_size_x The x size of the window, in meters + * @param win_size_y The y size of the window, in meters + */ + bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, + double win_size_y); + + /** + * @brief Default constructor + */ + Costmap2D(); + + /** + * @brief Destructor + */ + virtual ~Costmap2D(); + + /** + * @brief Get the cost of a cell in the costmap + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The cost of the cell + */ + unsigned char getCost(unsigned int mx, unsigned int my) const; + + /** + * @brief Set the cost of a cell in the costmap + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @param cost The cost to set the cell to + */ + void setCost(unsigned int mx, unsigned int my, unsigned char cost); + + /** + * @brief Convert from map coordinates to world coordinates + * @param mx The x map coordinate + * @param my The y map coordinate + * @param wx Will be set to the associated world x coordinate + * @param wy Will be set to the associated world y coordinate + */ + void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const; + + /** + * @brief Convert from world coordinates to map coordinates + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ + bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const; + + /** + * @brief Convert from world coordinates to map coordinates without checking for legal bounds + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @note The returned map coordinates are not guaranteed to lie within the map. + */ + void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const; + + /** + * @brief Convert from world coordinates to map coordinates, constraining results to legal bounds. + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @note The returned map coordinates are guaranteed to lie within the map. + */ + void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const; + + /** + * @brief Given two map coordinates... compute the associated index + * @param mx The x coordinate + * @param my The y coordinate + * @return The associated index + */ + inline unsigned int getIndex(unsigned int mx, unsigned int my) const + { + return my * size_x_ + mx; + } + + /** + * @brief Given an index... compute the associated map coordinates + * @param index The index + * @param mx Will be set to the x coordinate + * @param my Will be set to the y coordinate + */ + inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const + { + my = index / size_x_; + mx = index - (my * size_x_); + } + + /** + * @brief Will return a pointer to the underlying unsigned char array used as the costmap + * @return A pointer to the underlying unsigned char array storing cost values + */ + unsigned char* getCharMap() const; + + /** + * @brief Accessor for the x size of the costmap in cells + * @return The x size of the costmap + */ + unsigned int getSizeInCellsX() const; + + /** + * @brief Accessor for the y size of the costmap in cells + * @return The y size of the costmap + */ + unsigned int getSizeInCellsY() const; + + /** + * @brief Accessor for the x size of the costmap in meters + * @return The x size of the costmap (returns the centerpoint of the last legal cell in the map) + */ + double getSizeInMetersX() const; + + /** + * @brief Accessor for the y size of the costmap in meters + * @return The y size of the costmap (returns the centerpoint of the last legal cell in the map) + */ + double getSizeInMetersY() const; + + /** + * @brief Accessor for the x origin of the costmap + * @return The x origin of the costmap + */ + double getOriginX() const; + + /** + * @brief Accessor for the y origin of the costmap + * @return The y origin of the costmap + */ + double getOriginY() const; + + /** + * @brief Accessor for the resolution of the costmap + * @return The resolution of the costmap + */ + double getResolution() const; + + void setDefaultValue(unsigned char c) + { + default_value_ = c; + } + + unsigned char getDefaultValue() + { + return default_value_; + } + + /** + * @brief Sets the cost of a convex polygon to a desired value + * @param polygon The polygon to perform the operation on + * @param cost_value The value to set costs to + * @return True if the polygon was filled... false if it could not be filled + */ + bool setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value); + + /** + * @brief Get the map cells that make up the outline of a polygon + * @param polygon The polygon in map coordinates to rasterize + * @param polygon_cells Will be set to the cells contained in the outline of the polygon + */ + void polygonOutlineCells(const std::vector& polygon, std::vector& polygon_cells); + + /** + * @brief Get the map cells that fill a convex polygon + * @param polygon The polygon in map coordinates to rasterize + * @param polygon_cells Will be set to the cells that fill the polygon + */ + void convexFillCells(const std::vector& polygon, std::vector& polygon_cells); + + /** + * @brief Move the origin of the costmap to a new location.... keeping data when it can + * @param new_origin_x The x coordinate of the new origin + * @param new_origin_y The y coordinate of the new origin + */ + virtual void updateOrigin(double new_origin_x, double new_origin_y); + + /** + * @brief Save the costmap out to a pgm file + * @param file_name The name of the file to save + */ + bool saveMap(std::string file_name); + + void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, + double origin_y); + + void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn); + + /** + * @brief Given distance in the world... convert it to cells + * @param world_dist The world distance + * @return The equivalent cell distance + */ + unsigned int cellDistance(double world_dist); + + // Provide a typedef to ease future code maintenance + typedef boost::recursive_mutex mutex_t; + mutex_t* getMutex() + { + return access_; + } + +protected: + /** + * @brief Copy a region of a source map into a destination map + * @param source_map The source map + * @param sm_lower_left_x The lower left x point of the source map to start the copy + * @param sm_lower_left_y The lower left y point of the source map to start the copy + * @param sm_size_x The x size of the source map + * @param dest_map The destination map + * @param dm_lower_left_x The lower left x point of the destination map to start the copy + * @param dm_lower_left_y The lower left y point of the destination map to start the copy + * @param dm_size_x The x size of the destination map + * @param region_size_x The x size of the region to copy + * @param region_size_y The y size of the region to copy + */ + template + void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, + unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x, + unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, + unsigned int region_size_y) + { + // we'll first need to compute the starting points for each map + data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x); + data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x); + + // now, we'll copy the source map into the destination map + for (unsigned int i = 0; i < region_size_y; ++i) + { + memcpy(dm_index, sm_index, region_size_x * sizeof(data_type)); + sm_index += sm_size_x; + dm_index += dm_size_x; + } + } + + /** + * @brief Deletes the costmap, static_map, and markers data structures + */ + virtual void deleteMaps(); + + /** + * @brief Resets the costmap and static_map to be unknown space + */ + virtual void resetMaps(); + + /** + * @brief Initializes the costmap, static_map, and markers data structures + * @param size_x The x size to use for map initialization + * @param size_y The y size to use for map initialization + */ + virtual void initMaps(unsigned int size_x, unsigned int size_y); + + /** + * @brief Raytrace a line and apply some action at each step + * @param at The action to take... a functor + * @param x0 The starting x coordinate + * @param y0 The starting y coordinate + * @param x1 The ending x coordinate + * @param y1 The ending y coordinate + * @param max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint + */ + template + inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, + unsigned int max_length = UINT_MAX) + { + int dx = x1 - x0; + int dy = y1 - y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * size_x_; + + unsigned int offset = y0 * size_x_ + x0; + + // we need to chose how much to scale our dominant dimension, based on the maximum length of the line + double dist = hypot(dx, dy); + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + + // if x is dominant + if (abs_dx >= abs_dy) + { + int error_y = abs_dx / 2; + bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); + return; + } + + // otherwise y is dominant + int error_x = abs_dy / 2; + bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); + } + +private: + /** + * @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step + */ + template + inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, + int offset_b, unsigned int offset, unsigned int max_length) + { + unsigned int end = std::min(max_length, abs_da); + for (unsigned int i = 0; i < end; ++i) + { + at(offset); + offset += offset_a; + error_b += abs_db; + if ((unsigned int)error_b >= abs_da) + { + offset += offset_b; + error_b -= abs_da; + } + } + at(offset); + } + + inline int sign(int x) + { + return x > 0 ? 1.0 : -1.0; + } + + mutex_t* access_; +protected: + unsigned int size_x_; + unsigned int size_y_; + double resolution_; + double origin_x_; + double origin_y_; + unsigned char* costmap_; + unsigned char default_value_; + + class MarkCell + { + public: + MarkCell(unsigned char* costmap, unsigned char value) : + costmap_(costmap), value_(value) + { + } + inline void operator()(unsigned int offset) + { + costmap_[offset] = value_; + } + private: + unsigned char* costmap_; + unsigned char value_; + }; + + class PolygonOutlineCells + { + public: + PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector& cells) : + costmap_(costmap), char_map_(char_map), cells_(cells) + { + } + + // just push the relevant cells back onto the list + inline void operator()(unsigned int offset) + { + MapLocation loc; + costmap_.indexToCells(offset, loc.x, loc.y); + cells_.push_back(loc); + } + + private: + const Costmap2D& costmap_; + const unsigned char* char_map_; + std::vector& cells_; + }; +}; +} // namespace costmap_2d + +#endif // COSTMAP_2D_COSTMAP_2D_H diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_publisher.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_publisher.h new file mode 100644 index 0000000000..bf95569910 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_publisher.h @@ -0,0 +1,109 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_COSTMAP_2D_PUBLISHER_H_ +#define COSTMAP_2D_COSTMAP_2D_PUBLISHER_H_ +#include +#include +#include +#include +#include + +namespace costmap_2d +{ +/** + * @class Costmap2DPublisher + * @brief A tool to periodically publish visualization data from a Costmap2D + */ +class Costmap2DPublisher +{ +public: + /** + * @brief Constructor for the Costmap2DPublisher + */ + Costmap2DPublisher(ros::NodeHandle * ros_node, Costmap2D* costmap, std::string global_frame, + std::string topic_name, bool always_send_full_costmap = false); + + /** + * @brief Destructor + */ + ~Costmap2DPublisher(); + + /** @brief Include the given bounds in the changed-rectangle. */ + void updateBounds(unsigned int x0, unsigned int xn, unsigned int y0, unsigned int yn) + { + x0_ = std::min(x0, x0_); + xn_ = std::max(xn, xn_); + y0_ = std::min(y0, y0_); + yn_ = std::max(yn, yn_); + } + + /** + * @brief Publishes the visualization data over ROS + */ + void publishCostmap(); + + /** + * @brief Check if the publisher is active + * @return True if the frequency for the publisher is non-zero, false otherwise + */ + bool active() + { + return active_; + } + +private: + /** @brief Prepare grid_ message for publication. */ + void prepareGrid(); + + /** @brief Publish the latest full costmap to the new subscriber. */ + void onNewSubscription(const ros::SingleSubscriberPublisher& pub); + + ros::NodeHandle* node; + Costmap2D* costmap_; + std::string global_frame_; + unsigned int x0_, xn_, y0_, yn_; + double saved_origin_x_, saved_origin_y_; + bool active_; + bool always_send_full_costmap_; + ros::Publisher costmap_pub_; + ros::Publisher costmap_update_pub_; + nav_msgs::OccupancyGrid grid_; + static char* cost_translation_table_; ///< Translate from 0-255 values in costmap to -1 to 100 values in message. +}; +} // namespace costmap_2d +#endif // COSTMAP_2D_COSTMAP_2D_PUBLISHER_H diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_ros.h new file mode 100644 index 0000000000..4f9a02e59f --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_2d_ros.h @@ -0,0 +1,271 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_ +#define COSTMAP_2D_COSTMAP_2D_ROS_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SuperValue : public XmlRpc::XmlRpcValue +{ +public: + void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a) + { + _type = TypeStruct; + _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a); + } + void setArray(XmlRpc::XmlRpcValue::ValueArray* a) + { + _type = TypeArray; + _value.asArray = new std::vector(*a); + } +}; + +namespace costmap_2d +{ + +/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to + * topics that provide observations about obstacles in either the form + * of PointCloud or LaserScan messages. */ +class Costmap2DROS +{ +public: + /** + * @brief Constructor for the wrapper + * @param name The name for this costmap + * @param tf A reference to a TransformListener + */ + Costmap2DROS(std::string name, tf::TransformListener& tf); + ~Costmap2DROS(); + + /** + * @brief Subscribes to sensor topics if necessary and starts costmap + * updates, can be called to restart the costmap after calls to either + * stop() or pause() + */ + void start(); + + /** + * @brief Stops costmap updates and unsubscribes from sensor topics + */ + void stop(); + + /** + * @brief Stops the costmap from updating, but sensor data still comes in over the wire + */ + void pause(); + + /** + * @brief Resumes costmap updates + */ + void resume(); + + void updateMap(); + + /** + * @brief Reset each individual layer + */ + void resetLayers(); + + /** @brief Same as getLayeredCostmap()->isCurrent(). */ + bool isCurrent() + { + return layered_costmap_->isCurrent(); + } + + /** + * @brief Get the pose of the robot in the global frame of the costmap + * @param global_pose Will be set to the pose of the robot in the global frame of the costmap + * @return True if the pose was set successfully, false otherwise + */ + bool getRobotPose(tf::Stamped& global_pose) const; + + /** @brief Returns costmap name */ + std::string getName() const + { + return name_; + } + + /** @brief Returns the delay in transform (tf) data that is tolerable in seconds */ + double getTransformTolerance() const + { + return transform_tolerance_; + } + + /** @brief Return a pointer to the "master" costmap which receives updates from all the layers. + * + * Same as calling getLayeredCostmap()->getCostmap(). */ + Costmap2D* getCostmap() + { + return layered_costmap_->getCostmap(); + } + + /** + * @brief Returns the global frame of the costmap + * @return The global frame of the costmap + */ + std::string getGlobalFrameID() + { + return global_frame_; + } + + /** + * @brief Returns the local frame of the costmap + * @return The local frame of the costmap + */ + std::string getBaseFrameID() + { + return robot_base_frame_; + } + LayeredCostmap* getLayeredCostmap() + { + return layered_costmap_; + } + + /** @brief Returns the current padded footprint as a geometry_msgs::Polygon. */ + geometry_msgs::Polygon getRobotFootprintPolygon() + { + return costmap_2d::toPolygon(padded_footprint_); + } + + /** @brief Return the current footprint of the robot as a vector of points. + * + * This version of the footprint is padded by the footprint_padding_ + * distance, set in the rosparam "footprint_padding". + * + * The footprint initially comes from the rosparam "footprint" but + * can be overwritten by dynamic reconfigure or by messages received + * on the "footprint" topic. */ + std::vector getRobotFootprint() + { + return padded_footprint_; + } + + /** @brief Return the current unpadded footprint of the robot as a vector of points. + * + * This is the raw version of the footprint without padding. + * + * The footprint initially comes from the rosparam "footprint" but + * can be overwritten by dynamic reconfigure or by messages received + * on the "footprint" topic. */ + std::vector getUnpaddedRobotFootprint() + { + return unpadded_footprint_; + } + + /** + * @brief Build the oriented footprint of the robot at the robot's current pose + * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot + */ + void getOrientedFootprint(std::vector& oriented_footprint) const; + + /** @brief Set the footprint of the robot to be the given set of + * points, padded by footprint_padding. + * + * Should be a convex polygon, though this is not enforced. + * + * First expands the given polygon by footprint_padding_ and then + * sets padded_footprint_ and calls + * layered_costmap_->setFootprint(). Also saves the unpadded + * footprint, which is available from + * getUnpaddedRobotFootprint(). */ + void setUnpaddedRobotFootprint(const std::vector& points); + + /** @brief Set the footprint of the robot to be the given polygon, + * padded by footprint_padding. + * + * Should be a convex polygon, though this is not enforced. + * + * First expands the given polygon by footprint_padding_ and then + * sets padded_footprint_ and calls + * layered_costmap_->setFootprint(). Also saves the unpadded + * footprint, which is available from + * getUnpaddedRobotFootprint(). */ + void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint); + +protected: + LayeredCostmap* layered_costmap_; + std::string name_; + tf::TransformListener& tf_; ///< @brief Used for transforming point clouds + std::string global_frame_; ///< @brief The global frame for the costmap + std::string robot_base_frame_; ///< @brief The frame_id of the robot base + double transform_tolerance_; ///< timeout before transform errors + +private: + /** @brief Set the footprint from the new_config object. + * + * If the values of footprint and robot_radius are the same in + * new_config and old_config, nothing is changed. */ + void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, + const costmap_2d::Costmap2DConfig &old_config); + + void resetOldParameters(ros::NodeHandle& nh); + void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level); + void movementCB(const ros::TimerEvent &event); + void mapUpdateLoop(double frequency); + bool map_update_thread_shutdown_; + bool stop_updates_, initialized_, stopped_, robot_stopped_; + boost::thread* map_update_thread_; ///< @brief A thread for updating the map + ros::Timer timer_; + ros::Time last_publish_; + ros::Duration publish_cycle; + pluginlib::ClassLoader plugin_loader_; + tf::Stamped old_pose_; + Costmap2DPublisher* publisher_; + dynamic_reconfigure::Server *dsrv_; + + boost::recursive_mutex configuration_mutex_; + + ros::Subscriber footprint_sub_; + ros::Publisher footprint_pub_; + std::vector unpadded_footprint_; + std::vector padded_footprint_; + float footprint_padding_; + costmap_2d::Costmap2DConfig old_config_; +}; +// class Costmap2DROS +} // namespace costmap_2d + +#endif // COSTMAP_2D_COSTMAP_2D_ROS_H diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_layer.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_layer.h new file mode 100644 index 0000000000..d6237f9965 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_layer.h @@ -0,0 +1,149 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_COSTMAP_LAYER_H_ +#define COSTMAP_2D_COSTMAP_LAYER_H_ +#include +#include +#include + +namespace costmap_2d +{ + +class CostmapLayer : public Layer, public Costmap2D +{ +public: + CostmapLayer() : has_extra_bounds_(false), + extra_min_x_(1e6), extra_max_x_(-1e6), + extra_min_y_(1e6), extra_max_y_(-1e6) {} + + bool isDiscretized() + { + return true; + } + + virtual void matchSize(); + + /** + * If an external source changes values in the costmap, + * it should call this method with the area that it changed + * to ensure that the costmap includes this region as well. + * @param mx0 Minimum x value of the bounding box + * @param my0 Minimum y value of the bounding box + * @param mx1 Maximum x value of the bounding box + * @param my1 Maximum y value of the bounding box + */ + void addExtraBounds(double mx0, double my0, double mx1, double my1); + +protected: + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * TrueOverwrite means every value from this layer + * is written into the master grid. + */ + void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * Overwrite means every valid value from this layer + * is written into the master grid (does not copy NO_INFORMATION) + */ + void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * Sets the new value to the maximum of the master_grid's value + * and this layer's value. If the master value is NO_INFORMATION, + * it is overwritten. If the layer's value is NO_INFORMATION, + * the master value does not change. + */ + void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + /* + * Updates the master_grid within the specified + * bounding box using this layer's values. + * + * Sets the new value to the sum of the master grid's value + * and this layer's value. If the master value is NO_INFORMATION, + * it is overwritten with the layer's value. If the layer's value + * is NO_INFORMATION, then the master value does not change. + * + * If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE, + * the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1). + */ + void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + /** + * Updates the bounding box specified in the parameters to include + * the location (x,y) + * + * @param x x-coordinate to include + * @param y y-coordinate to include + * @param min_x bounding box + * @param min_y bounding box + * @param max_x bounding box + * @param max_y bounding box + */ + void touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y); + + /* + * Updates the bounding box specified in the parameters + * to include the bounding box from the addExtraBounds + * call. If addExtraBounds was not called, the method will do nothing. + * + * Should be called at the beginning of the updateBounds method + * + * @param min_x bounding box (input and output) + * @param min_y bounding box (input and output) + * @param max_x bounding box (input and output) + * @param max_y bounding box (input and output) + */ + void useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y); + bool has_extra_bounds_; + +private: + double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; +}; + +} // namespace costmap_2d +#endif // COSTMAP_2D_COSTMAP_LAYER_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_math.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_math.h new file mode 100644 index 0000000000..71fe96c45f --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/costmap_math.h @@ -0,0 +1,69 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_COSTMAP_MATH_H_ +#define COSTMAP_2D_COSTMAP_MATH_H_ + +#include +#include +#include +#include + +/** @brief Return -1 if x < 0, +1 otherwise. */ +inline double sign(double x) +{ + return x < 0.0 ? -1.0 : 1.0; +} + +/** @brief Same as sign(x) but returns 0 if x is 0. */ +inline double sign0(double x) +{ + return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); +} + +inline double distance(double x0, double y0, double x1, double y1) +{ + return hypot(x1 - x0, y1 - y0); +} + +double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1); + +bool intersects(std::vector& polygon, float testx, float testy); + +bool intersects(std::vector& polygon1, std::vector& polygon2); + +#endif // COSTMAP_2D_COSTMAP_MATH_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/footprint.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/footprint.h new file mode 100644 index 0000000000..6b1d1bec1b --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/footprint.h @@ -0,0 +1,147 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_FOOTPRINT_H +#define COSTMAP_2D_FOOTPRINT_H + +#include +#include +#include +#include +#include + +namespace costmap_2d +{ + +/** + * @brief Calculate the extreme distances for the footprint + * + * @param footprint The footprint to examine + * @param min_dist Output parameter of the minimum distance + * @param max_dist Output parameter of the maximum distance + */ +void calculateMinAndMaxDistances(const std::vector& footprint, + double& min_dist, double& max_dist); + +/** + * @brief Convert Point32 to Point + */ +geometry_msgs::Point toPoint(geometry_msgs::Point32 pt); + +/** + * @brief Convert Point to Point32 + */ +geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt); + +/** + * @brief Convert vector of Points to Polygon msg + */ +geometry_msgs::Polygon toPolygon(std::vector pts); + +/** + * @brief Convert Polygon msg to vector of Points. + */ +std::vector toPointVector(geometry_msgs::Polygon polygon); + +/** + * @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points) + * @param x The x position of the robot + * @param y The y position of the robot + * @param theta The orientation of the robot + * @param footprint_spec Basic shape of the footprint + * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot +*/ +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + std::vector& oriented_footprint); + +/** + * @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped) + * @param x The x position of the robot + * @param y The y position of the robot + * @param theta The orientation of the robot + * @param footprint_spec Basic shape of the footprint + * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot +*/ +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + geometry_msgs::PolygonStamped & oriented_footprint); + +/** + * @brief Adds the specified amount of padding to the footprint (in place) + */ +void padFootprint(std::vector& footprint, double padding); + +/** + * @brief Create a circular footprint from a given radius + */ +std::vector makeFootprintFromRadius(double radius); + +/** + * @brief Make the footprint from the given string. + * + * Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...] + * + */ +bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint); + +/** + * @brief Read the ros-params "footprint" and/or "robot_radius" from + * the given NodeHandle using searchParam() to go up the tree. + */ +std::vector makeFootprintFromParams(ros::NodeHandle& nh); + +/** + * @brief Create the footprint from the given XmlRpcValue. + * + * @param footprint_xmlrpc should be an array of arrays, where the + * top-level array should have 3 or more elements, and the + * sub-arrays should all have exactly 2 elements (x and y + * coordinates). + * + * @param full_param_name this is the full name of the rosparam from + * which the footprint_xmlrpc value came. It is used only for + * reporting errors. */ +std::vector makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, + const std::string& full_param_name); + +/** @brief Write the current unpadded_footprint_ to the "footprint" + * parameter of the given NodeHandle so that dynamic_reconfigure + * will see the new value. */ +void writeFootprintToParam(ros::NodeHandle& nh, const std::vector& footprint); + +} // end namespace costmap_2d + +#endif // COSTMAP_2D_FOOTPRINT_H diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/inflation_layer.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/inflation_layer.h new file mode 100644 index 0000000000..c04520b074 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/inflation_layer.h @@ -0,0 +1,198 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_INFLATION_LAYER_H_ +#define COSTMAP_2D_INFLATION_LAYER_H_ + +#include +#include +#include +#include +#include +#include + +namespace costmap_2d +{ +/** + * @class CellData + * @brief Storage for cell information used during obstacle inflation + */ +class CellData +{ +public: + /** + * @brief Constructor for a CellData objects + * @param i The index of the cell in the cost map + * @param x The x coordinate of the cell in the cost map + * @param y The y coordinate of the cell in the cost map + * @param sx The x coordinate of the closest obstacle cell in the costmap + * @param sy The y coordinate of the closest obstacle cell in the costmap + * @return + */ + CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : + index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) + { + } + unsigned int index_; + unsigned int x_, y_; + unsigned int src_x_, src_y_; +}; + +class InflationLayer : public Layer +{ +public: + InflationLayer(); + + virtual ~InflationLayer() + { + deleteKernels(); + if (dsrv_) + delete dsrv_; + } + + virtual void onInitialize(); + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + virtual bool isDiscretized() + { + return true; + } + virtual void matchSize(); + + virtual void reset() { onInitialize(); } + + /** @brief Given a distance, compute a cost. + * @param distance The distance from an obstacle in cells + * @return A cost value for the distance */ + virtual inline unsigned char computeCost(double distance) const + { + unsigned char cost = 0; + if (distance == 0) + cost = LETHAL_OBSTACLE; + else if (distance * resolution_ <= inscribed_radius_) + cost = INSCRIBED_INFLATED_OBSTACLE; + else + { + // make sure cost falls off by Euclidean distance + double euclidean_distance = distance * resolution_; + double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); + cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); + } + return cost; + } + + /** + * @brief Change the values of the inflation radius parameters + * @param inflation_radius The new inflation radius + * @param cost_scaling_factor The new weight + */ + void setInflationParameters(double inflation_radius, double cost_scaling_factor); + +protected: + virtual void onFootprintChanged(); + boost::recursive_mutex* inflation_access_; + + double resolution_; + double inflation_radius_; + double inscribed_radius_; + double weight_; + bool inflate_unknown_; + +private: + /** + * @brief Lookup pre-computed distances + * @param mx The x coordinate of the current cell + * @param my The y coordinate of the current cell + * @param src_x The x coordinate of the source cell + * @param src_y The y coordinate of the source cell + * @return + */ + inline double distanceLookup(int mx, int my, int src_x, int src_y) + { + unsigned int dx = abs(mx - src_x); + unsigned int dy = abs(my - src_y); + return cached_distances_[dx][dy]; + } + + /** + * @brief Lookup pre-computed costs + * @param mx The x coordinate of the current cell + * @param my The y coordinate of the current cell + * @param src_x The x coordinate of the source cell + * @param src_y The y coordinate of the source cell + * @return + */ + inline unsigned char costLookup(int mx, int my, int src_x, int src_y) + { + unsigned int dx = abs(mx - src_x); + unsigned int dy = abs(my - src_y); + return cached_costs_[dx][dy]; + } + + void computeCaches(); + void deleteKernels(); + void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid); + + unsigned int cellDistance(double world_dist) + { + return layered_costmap_->getCostmap()->cellDistance(world_dist); + } + + inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, + unsigned int src_x, unsigned int src_y); + + unsigned int cell_inflation_radius_; + unsigned int cached_cell_inflation_radius_; + std::map > inflation_cells_; + + bool* seen_; + int seen_size_; + + unsigned char** cached_costs_; + double** cached_distances_; + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + + dynamic_reconfigure::Server *dsrv_; + void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level); + + bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. +}; + +} // namespace costmap_2d + +#endif // COSTMAP_2D_INFLATION_LAYER_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layer.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layer.h new file mode 100644 index 0000000000..ac13cae3c8 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layer.h @@ -0,0 +1,134 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_LAYER_H_ +#define COSTMAP_2D_LAYER_H_ + +#include +#include +#include +#include +#include + +namespace costmap_2d +{ +class LayeredCostmap; + +class Layer +{ +public: + Layer(); + + void initialize(LayeredCostmap* parent, std::string name, tf::TransformListener *tf); + + /** + * @brief This is called by the LayeredCostmap to poll this plugin as to how + * much of the costmap it needs to update. Each layer can increase + * the size of this bounds. + * + * For more details, see "Layered Costmaps for Context-Sensitive Navigation", + * by Lu et. Al, IROS 2014. + */ + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y) {} + + /** + * @brief Actually update the underlying costmap, only within the bounds + * calculated during UpdateBounds(). + */ + virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {} + + /** @brief Stop publishers. */ + virtual void deactivate() {} + + /** @brief Restart publishers if they've been stopped. */ + virtual void activate() {} + + virtual void reset() {} + + virtual ~Layer() {} + + /** + * @brief Check to make sure all the data in the layer is up to date. + * If the layer is not up to date, then it may be unsafe to + * plan using the data from this layer, and the planner may + * need to know. + * + * A layer's current state should be managed by the protected + * variable current_. + * @return Whether the data in the layer is up to date. + */ + bool isCurrent() const + { + return current_; + } + + /** @brief Implement this to make this layer match the size of the parent costmap. */ + virtual void matchSize() {} + + std::string getName() const + { + return name_; + } + + /** @brief Convenience function for layered_costmap_->getFootprint(). */ + const std::vector& getFootprint() const; + + /** @brief LayeredCostmap calls this whenever the footprint there + * changes (via LayeredCostmap::setFootprint()). Override to be + * notified of changes to the robot's footprint. */ + virtual void onFootprintChanged() {} + +protected: + /** @brief This is called at the end of initialize(). Override to + * implement subclass-specific initialization. + * + * tf_, name_, and layered_costmap_ will all be set already when this is called. */ + virtual void onInitialize() {} + + LayeredCostmap* layered_costmap_; + bool current_; + bool enabled_; ///< Currently this var is managed by subclasses. TODO: make this managed by this class and/or container class. + std::string name_; + tf::TransformListener* tf_; + +private: + std::vector footprint_spec_; +}; + +} // namespace costmap_2d + +#endif // COSTMAP_2D_LAYER_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layered_costmap.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layered_costmap.h new file mode 100644 index 0000000000..495d9c3002 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/layered_costmap.h @@ -0,0 +1,177 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_ +#define COSTMAP_2D_LAYERED_COSTMAP_H_ + +#include +#include +#include +#include +#include + +namespace costmap_2d +{ +class Layer; + +/** + * @class LayeredCostmap + * @brief Instantiates different layer plugins and aggregates them into one score + */ +class LayeredCostmap +{ +public: + /** + * @brief Constructor for a costmap + */ + LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown); + + /** + * @brief Destructor + */ + ~LayeredCostmap(); + + /** + * @brief Update the underlying costmap with new data. + * If you want to update the map outside of the update loop that runs, you can call this. + */ + void updateMap(double robot_x, double robot_y, double robot_yaw); + + std::string getGlobalFrameID() const + { + return global_frame_; + } + + void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, + bool size_locked = false); + + void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy) + { + minx = minx_; + miny = miny_; + maxx = maxx_; + maxy = maxy_; + } + + bool isCurrent(); + + Costmap2D* getCostmap() + { + return &costmap_; + } + + bool isRolling() + { + return rolling_window_; + } + + bool isTrackingUnknown() + { + return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION; + } + + std::vector >* getPlugins() + { + return &plugins_; + } + + void addPlugin(boost::shared_ptr plugin) + { + plugins_.push_back(plugin); + } + + bool isSizeLocked() + { + return size_locked_; + } + + void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn) + { + *x0 = bx0_; + *xn = bxn_; + *y0 = by0_; + *yn = byn_; + } + + bool isInitialized() + { + return initialized_; + } + + /** @brief Updates the stored footprint, updates the circumscribed + * and inscribed radii, and calls onFootprintChanged() in all + * layers. */ + void setFootprint(const std::vector& footprint_spec); + + /** @brief Returns the latest footprint stored with setFootprint(). */ + const std::vector& getFootprint() { return footprint_; } + + /** @brief The radius of a circle centered at the origin of the + * robot which just surrounds all points on the robot's + * footprint. + * + * This is updated by setFootprint(). */ + double getCircumscribedRadius() { return circumscribed_radius_; } + + /** @brief The radius of a circle centered at the origin of the + * robot which is just within all points and edges of the robot's + * footprint. + * + * This is updated by setFootprint(). */ + double getInscribedRadius() { return inscribed_radius_; } + +private: + Costmap2D costmap_; + std::string global_frame_; + + bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot + + bool current_; + double minx_, miny_, maxx_, maxy_; + unsigned int bx0_, bxn_, by0_, byn_; + + std::vector > plugins_; + + bool initialized_; + bool size_locked_; + double circumscribed_radius_, inscribed_radius_; + std::vector footprint_; +}; + +} // namespace costmap_2d + +#endif // COSTMAP_2D_LAYERED_COSTMAP_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation.h new file mode 100644 index 0000000000..8b72ce7cbd --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation.h @@ -0,0 +1,103 @@ +/* + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: Conor McGann + */ + +#ifndef COSTMAP_2D_OBSERVATION_H_ +#define COSTMAP_2D_OBSERVATION_H_ + +#include +#include +#include + +namespace costmap_2d +{ + +/** + * @brief Stores an observation in terms of a point cloud and the origin of the source + * @note Tried to make members and constructor arguments const but the compiler would not accept the default + * assignment operator for vector insertion! + */ +class Observation +{ +public: + /** + * @brief Creates an empty observation + */ + Observation() : + cloud_(new pcl::PointCloud()), obstacle_range_(0.0), raytrace_range_(0.0) + { + } + + virtual ~Observation() + { + delete cloud_; + } + + /** + * @brief Creates an observation from an origin point and a point cloud + * @param origin The origin point of the observation + * @param cloud The point cloud of the observation + * @param obstacle_range The range out to which an observation should be able to insert obstacles + * @param raytrace_range The range out to which an observation should be able to clear via raytracing + */ + Observation(geometry_msgs::Point& origin, pcl::PointCloud cloud, + double obstacle_range, double raytrace_range) : + origin_(origin), cloud_(new pcl::PointCloud(cloud)), + obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) + { + } + + /** + * @brief Copy constructor + * @param obs The observation to copy + */ + Observation(const Observation& obs) : + origin_(obs.origin_), cloud_(new pcl::PointCloud(*(obs.cloud_))), + obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_) + { + } + + /** + * @brief Creates an observation from a point cloud + * @param cloud The point cloud of the observation + * @param obstacle_range The range out to which an observation should be able to insert obstacles + */ + Observation(pcl::PointCloud cloud, double obstacle_range) : + cloud_(new pcl::PointCloud(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0) + { + } + + geometry_msgs::Point origin_; + pcl::PointCloud* cloud_; + double obstacle_range_, raytrace_range_; +}; + +} // namespace costmap_2d +#endif // COSTMAP_2D_OBSERVATION_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation_buffer.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation_buffer.h new file mode 100644 index 0000000000..8f7c98abd3 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/observation_buffer.h @@ -0,0 +1,164 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_ +#define COSTMAP_2D_OBSERVATION_BUFFER_H_ + +#include +#include +#include +#include +#include +#include +#include + +// PCL Stuff +#include +#include + +// Thread support +#include + +namespace costmap_2d +{ +/** + * @class ObservationBuffer + * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them + */ +class ObservationBuffer +{ +public: + /** + * @brief Constructs an observation buffer + * @param topic_name The topic of the observations, used as an identifier for error and warning messages + * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest + * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit + * @param min_obstacle_height The minimum height of a hitpoint to be considered legal + * @param max_obstacle_height The minimum height of a hitpoint to be considered legal + * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles + * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space + * @param tf A reference to a TransformListener + * @param global_frame The frame to transform PointClouds into + * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages + * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame + */ + ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate, + double min_obstacle_height, double max_obstacle_height, double obstacle_range, + double raytrace_range, tf::TransformListener& tf, std::string global_frame, + std::string sensor_frame, double tf_tolerance); + + /** + * @brief Destructor... cleans up + */ + ~ObservationBuffer(); + + /** + * @brief Sets the global frame of an observation buffer. This will + * transform all the currently cached observations to the new global + * frame + * @param new_global_frame The name of the new global frame. + * @return True if the operation succeeds, false otherwise + */ + bool setGlobalFrame(const std::string new_global_frame); + + /** + * @brief Transforms a PointCloud to the global frame and buffers it + * Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier + * @param cloud The cloud to be buffered + */ + void bufferCloud(const sensor_msgs::PointCloud2& cloud); + + /** + * @brief Transforms a PointCloud to the global frame and buffers it + * Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier + * @param cloud The cloud to be buffered + */ + void bufferCloud(const pcl::PointCloud& cloud); + + /** + * @brief Pushes copies of all current observations onto the end of the vector passed in + * @param observations The vector to be filled + */ + void getObservations(std::vector& observations); + + /** + * @brief Check if the observation buffer is being update at its expected rate + * @return True if it is being updated at the expected rate, false otherwise + */ + bool isCurrent() const; + + /** + * @brief Lock the observation buffer + */ + inline void lock() + { + lock_.lock(); + } + + /** + * @brief Lock the observation buffer + */ + inline void unlock() + { + lock_.unlock(); + } + + /** + * @brief Reset last updated timestamp + */ + void resetLastUpdated(); + +private: + /** + * @brief Removes any stale observations from the buffer list + */ + void purgeStaleObservations(); + + tf::TransformListener& tf_; + const ros::Duration observation_keep_time_; + const ros::Duration expected_update_rate_; + ros::Time last_updated_; + std::string global_frame_; + std::string sensor_frame_; + std::list observation_list_; + std::string topic_name_; + double min_obstacle_height_, max_obstacle_height_; + boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely + double obstacle_range_, raytrace_range_; + double tf_tolerance_; +}; +} // namespace costmap_2d +#endif // COSTMAP_2D_OBSERVATION_BUFFER_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/obstacle_layer.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/obstacle_layer.h new file mode 100644 index 0000000000..a70691a666 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/obstacle_layer.h @@ -0,0 +1,177 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_ +#define COSTMAP_2D_OBSTACLE_LAYER_H_ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace costmap_2d +{ + +class ObstacleLayer : public CostmapLayer +{ +public: + ObstacleLayer() + { + costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D. + } + + virtual ~ObstacleLayer(); + virtual void onInitialize(); + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + virtual void activate(); + virtual void deactivate(); + virtual void reset(); + + /** + * @brief A callback to handle buffering LaserScan messages + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, + const boost::shared_ptr& buffer); + + /** + * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& message, + const boost::shared_ptr& buffer); + + /** + * @brief A callback to handle buffering PointCloud messages + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, + const boost::shared_ptr& buffer); + + /** + * @brief A callback to handle buffering PointCloud2 messages + * @param message The message returned from a message notifier + * @param buffer A pointer to the observation buffer to update + */ + void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, + const boost::shared_ptr& buffer); + + // for testing purposes + void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing); + void clearStaticObservations(bool marking, bool clearing); + +protected: + virtual void setupDynamicReconfigure(ros::NodeHandle& nh); + + /** + * @brief Get the observations used to mark space + * @param marking_observations A reference to a vector that will be populated with the observations + * @return True if all the observation buffers are current, false otherwise + */ + bool getMarkingObservations(std::vector& marking_observations) const; + + /** + * @brief Get the observations used to clear space + * @param clearing_observations A reference to a vector that will be populated with the observations + * @return True if all the observation buffers are current, false otherwise + */ + bool getClearingObservations(std::vector& clearing_observations) const; + + /** + * @brief Clear freespace based on one observation + * @param clearing_observation The observation used to raytrace + * @param min_x + * @param min_y + * @param max_x + * @param max_y + */ + virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, + double* max_x, double* max_y); + + void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y, + double* max_x, double* max_y); + + std::vector transformed_footprint_; + bool footprint_clearing_enabled_; + void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + + std::string global_frame_; ///< @brief The global frame for the costmap + double max_obstacle_height_; ///< @brief Max Obstacle Height + + laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds + + std::vector > observation_subscribers_; ///< @brief Used for the observation message filters + std::vector > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor + std::vector > observation_buffers_; ///< @brief Used to store observations from various sensors + std::vector > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles + std::vector > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles + + // Used only for testing purposes + std::vector static_clearing_observations_, static_marking_observations_; + + bool rolling_window_; + dynamic_reconfigure::Server *dsrv_; + + int combination_method_; + +private: + void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level); +}; + +} // namespace costmap_2d + +#endif // COSTMAP_2D_OBSTACLE_LAYER_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/static_layer.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/static_layer.h new file mode 100644 index 0000000000..3fe9a76464 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/static_layer.h @@ -0,0 +1,101 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_STATIC_LAYER_H_ +#define COSTMAP_2D_STATIC_LAYER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace costmap_2d +{ + +class StaticLayer : public CostmapLayer +{ +public: + StaticLayer(); + virtual ~StaticLayer(); + virtual void onInitialize(); + virtual void activate(); + virtual void deactivate(); + virtual void reset(); + + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + virtual void matchSize(); + +private: + /** + * @brief Callback to update the costmap's map from the map_server + * @param new_map The map to put into the costmap. The origin of the new + * map along with its size will determine what parts of the costmap's + * static map are overwritten. + */ + void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map); + void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update); + void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level); + + unsigned char interpretValue(unsigned char value); + + std::string global_frame_; ///< @brief The global frame for the costmap + std::string map_frame_; /// @brief frame that map is located in + bool subscribe_to_updates_; + bool map_received_; + bool has_updated_data_; + unsigned int x_, y_, width_, height_; + bool track_unknown_space_; + bool use_maximum_; + bool first_map_only_; ///< @brief Store the first static map and reuse it on reinitializing + bool trinary_costmap_; + ros::Subscriber map_sub_, map_update_sub_; + + unsigned char lethal_threshold_, unknown_cost_value_; + + dynamic_reconfigure::Server *dsrv_; +}; + +} // namespace costmap_2d + +#endif // COSTMAP_2D_STATIC_LAYER_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/testing_helper.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/testing_helper.h new file mode 100644 index 0000000000..ee168ff708 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/testing_helper.h @@ -0,0 +1,102 @@ +#ifndef COSTMAP_2D_TESTING_HELPER_H +#define COSTMAP_2D_TESTING_HELPER_H + +#include +#include +#include +#include +#include + +const double MAX_Z(1.0); + +void setValues(costmap_2d::Costmap2D& costmap, const unsigned char* map) +{ + int index = 0; + for (int i = 0; i < costmap.getSizeInCellsY(); i++){ + for (int j = 0; j < costmap.getSizeInCellsX(); j++){ + costmap.setCost(j, i, map[index]); + } + } +} + +char printableCost(unsigned char cost) +{ + switch (cost) + { + case costmap_2d::NO_INFORMATION: return '?'; + case costmap_2d::LETHAL_OBSTACLE: return 'L'; + case costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I'; + case costmap_2d::FREE_SPACE: return '.'; + default: return '0' + (unsigned char) (10 * cost / 255); + } +} + +void printMap(costmap_2d::Costmap2D& costmap) +{ + printf("map:\n"); + for (int i = 0; i < costmap.getSizeInCellsY(); i++){ + for (int j = 0; j < costmap.getSizeInCellsX(); j++){ + printf("%4d", int(costmap.getCost(j, i))); + } + printf("\n\n"); + } +} + +unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true) +{ + unsigned int count = 0; + for (int i = 0; i < costmap.getSizeInCellsY(); i++){ + for (int j = 0; j < costmap.getSizeInCellsX(); j++){ + unsigned char c = costmap.getCost(j, i); + if ((equal && c == value) || (!equal && c != value)) + { + count+=1; + } + } + } + return count; +} + +void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf::TransformListener& tf) +{ + costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer(); + layers.addPlugin(boost::shared_ptr(slayer)); + slayer->initialize(&layers, "static", &tf); +} + +costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf::TransformListener& tf) +{ + costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer(); + olayer->initialize(&layers, "obstacles", &tf); + layers.addPlugin(boost::shared_ptr(olayer)); + return olayer; +} + +void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0, + double ox = 0.0, double oy = 0.0, double oz = MAX_Z){ + pcl::PointCloud cloud; + cloud.points.resize(1); + cloud.points[0].x = x; + cloud.points[0].y = y; + cloud.points[0].z = z; + + geometry_msgs::Point p; + p.x = ox; + p.y = oy; + p.z = oz; + + costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0 + olayer->addStaticObservation(obs, true, true); +} + +costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf::TransformListener& tf) +{ + costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer(); + ilayer->initialize(&layers, "inflation", &tf); + boost::shared_ptr ipointer(ilayer); + layers.addPlugin(ipointer); + return ilayer; +} + + +#endif // COSTMAP_2D_TESTING_HELPER_H diff --git a/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/voxel_layer.h b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/voxel_layer.h new file mode 100644 index 0000000000..f5038d37a1 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/include/costmap_2d/voxel_layer.h @@ -0,0 +1,151 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_VOXEL_LAYER_H_ +#define COSTMAP_2D_VOXEL_LAYER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace costmap_2d +{ + +class VoxelLayer : public ObstacleLayer +{ +public: + VoxelLayer() : + voxel_grid_(0, 0, 0) + { + costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D. + } + + virtual ~VoxelLayer(); + + virtual void onInitialize(); + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + + void updateOrigin(double new_origin_x, double new_origin_y); + bool isDiscretized() + { + return true; + } + virtual void matchSize(); + virtual void reset(); + + +protected: + virtual void setupDynamicReconfigure(ros::NodeHandle& nh); + + virtual void resetMaps(); + +private: + void reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level); + void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info); + virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, + double* max_x, double* max_y); + + dynamic_reconfigure::Server *voxel_dsrv_; + + bool publish_voxel_; + ros::Publisher voxel_pub_; + voxel_grid::VoxelGrid voxel_grid_; + double z_resolution_, origin_z_; + unsigned int unknown_threshold_, mark_threshold_, size_z_; + ros::Publisher clearing_endpoints_pub_; + sensor_msgs::PointCloud clearing_endpoints_; + + inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz) + { + if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) + return false; + mx = ((wx - origin_x_) / resolution_); + my = ((wy - origin_y_) / resolution_); + mz = ((wz - origin_z_) / z_resolution_); + if (mx < size_x_ && my < size_y_ && mz < size_z_) + return true; + + return false; + } + + inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz) + { + if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) + return false; + + mx = (int)((wx - origin_x_) / resolution_); + my = (int)((wy - origin_y_) / resolution_); + mz = (int)((wz - origin_z_) / z_resolution_); + + if (mx < size_x_ && my < size_y_ && mz < size_z_) + return true; + + return false; + } + + inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz) + { + // returns the center point of the cell + wx = origin_x_ + (mx + 0.5) * resolution_; + wy = origin_y_ + (my + 0.5) * resolution_; + wz = origin_z_ + (mz + 0.5) * z_resolution_; + } + + inline double dist(double x0, double y0, double z0, double x1, double y1, double z1) + { + return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0)); + } +}; + +} // namespace costmap_2d + +#endif // COSTMAP_2D_VOXEL_LAYER_H_ diff --git a/src/libs/nav2_util/src/costmap_2d/launch/example.launch b/src/libs/nav2_util/src/costmap_2d/launch/example.launch new file mode 100644 index 0000000000..87cd2ea120 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/launch/example.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + diff --git a/src/libs/nav2_util/src/costmap_2d/launch/example_params.yaml b/src/libs/nav2_util/src/costmap_2d/launch/example_params.yaml new file mode 100644 index 0000000000..05d3dd7cd7 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/launch/example_params.yaml @@ -0,0 +1,40 @@ +global_frame: /map +robot_base_frame: base_link +update_frequency: 5.0 +publish_frequency: 1.0 + +#set if you want the voxel map published +publish_voxel_map: true + +#set to true if you want to initialize the costmap from a static map +static_map: false + +#begin - COMMENT these lines if you set static_map to true +rolling_window: true +width: 6.0 +height: 6.0 +resolution: 0.025 +#end - COMMENT these lines if you set static_map to true + +#START VOXEL STUFF +map_type: voxel +origin_z: 0.0 +z_resolution: 0.2 +z_voxels: 10 +unknown_threshold: 10 +mark_threshold: 0 +#END VOXEL STUFF + +transform_tolerance: 0.3 +obstacle_range: 2.5 +max_obstacle_height: 2.0 +raytrace_range: 3.0 +footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] +#robot_radius: 0.46 +footprint_padding: 0.01 +inflation_radius: 0.55 +cost_scaling_factor: 10.0 +lethal_cost_threshold: 100 +observation_sources: base_scan +base_scan: {data_type: LaserScan, expected_update_rate: 0.4, + observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} diff --git a/src/libs/nav2_util/src/costmap_2d/msg/VoxelGrid.msg b/src/libs/nav2_util/src/costmap_2d/msg/VoxelGrid.msg new file mode 100644 index 0000000000..01eb573f4b --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/msg/VoxelGrid.msg @@ -0,0 +1,8 @@ +Header header +uint32[] data +geometry_msgs/Point32 origin +geometry_msgs/Vector3 resolutions +uint32 size_x +uint32 size_y +uint32 size_z + diff --git a/src/libs/nav2_util/src/costmap_2d/package.xml b/src/libs/nav2_util/src/costmap_2d/package.xml new file mode 100644 index 0000000000..fae5b74ce4 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/package.xml @@ -0,0 +1,59 @@ + + + + costmap_2d + 1.15.2 + + This package provides an implementation of a 2D costmap that takes in sensor + data from the world, builds a 2D or 3D occupancy grid of the data (depending + on whether a voxel based implementation is used), and inflates costs in a + 2D costmap based on the occupancy grid and a user specified inflation radius. + This package also provides support for map_server based initialization of a + costmap, rolling window based costmaps, and parameter based subscription to + and configuration of sensor topics. + + Eitan Marder-Eppstein + David V. Lu!! + Dave Hershberger + contradict@gmail.com + David V. Lu!! + Michael Ferguson + Aaron Hoy + BSD + http://wiki.ros.org/costmap_2d + + catkin + + cmake_modules + message_generation + + dynamic_reconfigure + geometry_msgs + laser_geometry + map_msgs + message_filters + nav_msgs + pcl_conversions + pcl_ros + pluginlib + libpcl-all-dev + roscpp + rostest + sensor_msgs + std_msgs + tf + visualization_msgs + voxel_grid + + message_runtime + rosconsole + + map_server + rosbag + rostest + rosunit + + + + + diff --git a/src/libs/nav2_util/src/costmap_2d/plugins/inflation_layer.cpp b/src/libs/nav2_util/src/costmap_2d/plugins/inflation_layer.cpp new file mode 100644 index 0000000000..e06021a9d3 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/plugins/inflation_layer.cpp @@ -0,0 +1,383 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer) + +using costmap_2d::LETHAL_OBSTACLE; +using costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using costmap_2d::NO_INFORMATION; + +namespace costmap_2d +{ + +InflationLayer::InflationLayer() + : inflation_radius_(0) + , weight_(0) + , inflate_unknown_(false) + , cell_inflation_radius_(0) + , cached_cell_inflation_radius_(0) + , dsrv_(NULL) + , seen_(NULL) + , cached_costs_(NULL) + , cached_distances_(NULL) + , last_min_x_(-std::numeric_limits::max()) + , last_min_y_(-std::numeric_limits::max()) + , last_max_x_(std::numeric_limits::max()) + , last_max_y_(std::numeric_limits::max()) +{ + inflation_access_ = new boost::recursive_mutex(); +} + +void InflationLayer::onInitialize() +{ + { + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + ros::NodeHandle nh("~/" + name_), g_nh; + current_ = true; + if (seen_) + delete[] seen_; + seen_ = NULL; + seen_size_ = 0; + need_reinflation_ = false; + + dynamic_reconfigure::Server::CallbackType cb = boost::bind( + &InflationLayer::reconfigureCB, this, _1, _2); + + if (dsrv_ != NULL){ + dsrv_->clearCallback(); + dsrv_->setCallback(cb); + } + else + { + dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name_)); + dsrv_->setCallback(cb); + } + } + + matchSize(); +} + +void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level) +{ + setInflationParameters(config.inflation_radius, config.cost_scaling_factor); + + if (enabled_ != config.enabled || inflate_unknown_ != config.inflate_unknown) { + enabled_ = config.enabled; + inflate_unknown_ = config.inflate_unknown; + need_reinflation_ = true; + } +} + +void InflationLayer::matchSize() +{ + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); + resolution_ = costmap->getResolution(); + cell_inflation_radius_ = cellDistance(inflation_radius_); + computeCaches(); + + unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY(); + if (seen_) + delete[] seen_; + seen_size_ = size_x * size_y; + seen_ = new bool[seen_size_]; +} + +void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, + double* min_y, double* max_x, double* max_y) +{ + if (need_reinflation_) + { + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + // For some reason when I make these -::max() it does not + // work with Costmap2D::worldToMapEnforceBounds(), so I'm using + // -::max() instead. + *min_x = -std::numeric_limits::max(); + *min_y = -std::numeric_limits::max(); + *max_x = std::numeric_limits::max(); + *max_y = std::numeric_limits::max(); + need_reinflation_ = false; + } + else + { + double tmp_min_x = last_min_x_; + double tmp_min_y = last_min_y_; + double tmp_max_x = last_max_x_; + double tmp_max_y = last_max_y_; + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_; + *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_; + *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_; + *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_; + } +} + +void InflationLayer::onFootprintChanged() +{ + inscribed_radius_ = layered_costmap_->getInscribedRadius(); + cell_inflation_radius_ = cellDistance(inflation_radius_); + computeCaches(); + need_reinflation_ = true; + + ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu," + " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", + layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); +} + +void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + if (!enabled_ || (cell_inflation_radius_ == 0)) + return; + + // make sure the inflation list is empty at the beginning of the cycle (should always be true) + ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation"); + + unsigned char* master_array = master_grid.getCharMap(); + unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); + + if (seen_ == NULL) { + ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL"); + seen_size_ = size_x * size_y; + seen_ = new bool[seen_size_]; + } + else if (seen_size_ != size_x * size_y) + { + ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong"); + delete[] seen_; + seen_size_ = size_x * size_y; + seen_ = new bool[seen_size_]; + } + memset(seen_, false, size_x * size_y * sizeof(bool)); + + // We need to include in the inflation cells outside the bounding + // box min_i...max_j, by the amount cell_inflation_radius_. Cells + // up to that distance outside the box can still influence the costs + // stored in cells inside the box. + min_i -= cell_inflation_radius_; + min_j -= cell_inflation_radius_; + max_i += cell_inflation_radius_; + max_j += cell_inflation_radius_; + + min_i = std::max(0, min_i); + min_j = std::max(0, min_j); + max_i = std::min(int(size_x), max_i); + max_j = std::min(int(size_y), max_j); + + // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle + // We use a map to emulate the priority queue used before, with a notable performance boost + + // Start with lethal obstacles: by definition distance is 0.0 + std::vector& obs_bin = inflation_cells_[0.0]; + for (int j = min_j; j < max_j; j++) + { + for (int i = min_i; i < max_i; i++) + { + int index = master_grid.getIndex(i, j); + unsigned char cost = master_array[index]; + if (cost == LETHAL_OBSTACLE) + { + obs_bin.push_back(CellData(index, i, j, i, j)); + } + } + } + + // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they + // can overtake previously inserted but farther away cells + std::map >::iterator bin; + for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) + { + for (int i = 0; i < bin->second.size(); ++i) + { + // process all cells at distance dist_bin.first + const CellData& cell = bin->second[i]; + + unsigned int index = cell.index_; + + // ignore if already visited + if (seen_[index]) + { + continue; + } + + seen_[index] = true; + + unsigned int mx = cell.x_; + unsigned int my = cell.y_; + unsigned int sx = cell.src_x_; + unsigned int sy = cell.src_y_; + + // assign the cost associated with the distance from an obstacle to the cell + unsigned char cost = costLookup(mx, my, sx, sy); + unsigned char old_cost = master_array[index]; + if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) + master_array[index] = cost; + else + master_array[index] = std::max(old_cost, cost); + + // attempt to put the neighbors of the current cell onto the inflation list + if (mx > 0) + enqueue(index - 1, mx - 1, my, sx, sy); + if (my > 0) + enqueue(index - size_x, mx, my - 1, sx, sy); + if (mx < size_x - 1) + enqueue(index + 1, mx + 1, my, sx, sy); + if (my < size_y - 1) + enqueue(index + size_x, mx, my + 1, sx, sy); + } + } + + inflation_cells_.clear(); +} + +/** + * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation + * @param grid The costmap + * @param index The index of the cell + * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) + * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) + * @param src_x The x index of the obstacle point inflation started at + * @param src_y The y index of the obstacle point inflation started at + */ +inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my, + unsigned int src_x, unsigned int src_y) +{ + if (!seen_[index]) + { + // we compute our distance table one cell further than the inflation radius dictates so we can make the check below + double distance = distanceLookup(mx, my, src_x, src_y); + + // we only want to put the cell in the list if it is within the inflation radius of the obstacle point + if (distance > cell_inflation_radius_) + return; + + // push the cell data onto the inflation list and mark + inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); + } +} + +void InflationLayer::computeCaches() +{ + if (cell_inflation_radius_ == 0) + return; + + // based on the inflation radius... compute distance and cost caches + if (cell_inflation_radius_ != cached_cell_inflation_radius_) + { + deleteKernels(); + + cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; + cached_distances_ = new double*[cell_inflation_radius_ + 2]; + + for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) + { + cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; + cached_distances_[i] = new double[cell_inflation_radius_ + 2]; + for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) + { + cached_distances_[i][j] = hypot(i, j); + } + } + + cached_cell_inflation_radius_ = cell_inflation_radius_; + } + + for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) + { + for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) + { + cached_costs_[i][j] = computeCost(cached_distances_[i][j]); + } + } +} + +void InflationLayer::deleteKernels() +{ + if (cached_distances_ != NULL) + { + for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) + { + if (cached_distances_[i]) + delete[] cached_distances_[i]; + } + if (cached_distances_) + delete[] cached_distances_; + cached_distances_ = NULL; + } + + if (cached_costs_ != NULL) + { + for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) + { + if (cached_costs_[i]) + delete[] cached_costs_[i]; + } + delete[] cached_costs_; + cached_costs_ = NULL; + } +} + +void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor) +{ + if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius) + { + // Lock here so that reconfiguring the inflation radius doesn't cause segfaults + // when accessing the cached arrays + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + + inflation_radius_ = inflation_radius; + cell_inflation_radius_ = cellDistance(inflation_radius_); + weight_ = cost_scaling_factor; + need_reinflation_ = true; + computeCaches(); + } +} + +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/plugins/obstacle_layer.cpp b/src/libs/nav2_util/src/costmap_2d/plugins/obstacle_layer.cpp new file mode 100644 index 0000000000..5eac06308c --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/plugins/obstacle_layer.cpp @@ -0,0 +1,620 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer) + +using costmap_2d::NO_INFORMATION; +using costmap_2d::LETHAL_OBSTACLE; +using costmap_2d::FREE_SPACE; + +using costmap_2d::ObservationBuffer; +using costmap_2d::Observation; + +namespace costmap_2d +{ + +void ObstacleLayer::onInitialize() +{ + ros::NodeHandle nh("~/" + name_), g_nh; + rolling_window_ = layered_costmap_->isRolling(); + + bool track_unknown_space; + nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); + if (track_unknown_space) + default_value_ = NO_INFORMATION; + else + default_value_ = FREE_SPACE; + + ObstacleLayer::matchSize(); + current_ = true; + + global_frame_ = layered_costmap_->getGlobalFrameID(); + double transform_tolerance; + nh.param("transform_tolerance", transform_tolerance, 0.2); + + std::string topics_string; + // get the topics that we'll subscribe to from the parameter server + nh.param("observation_sources", topics_string, std::string("")); + ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str()); + + // get our tf prefix + ros::NodeHandle prefix_nh; + const std::string tf_prefix = tf::getPrefixParam(prefix_nh); + + // now we need to split the topics based on whitespace which we can use a stringstream for + std::stringstream ss(topics_string); + + std::string source; + while (ss >> source) + { + ros::NodeHandle source_node(nh, source); + + // get the parameters for the specific topic + double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; + std::string topic, sensor_frame, data_type; + bool inf_is_valid, clearing, marking; + + source_node.param("topic", topic, source); + source_node.param("sensor_frame", sensor_frame, std::string("")); + source_node.param("observation_persistence", observation_keep_time, 0.0); + source_node.param("expected_update_rate", expected_update_rate, 0.0); + source_node.param("data_type", data_type, std::string("PointCloud")); + source_node.param("min_obstacle_height", min_obstacle_height, 0.0); + source_node.param("max_obstacle_height", max_obstacle_height, 2.0); + source_node.param("inf_is_valid", inf_is_valid, false); + source_node.param("clearing", clearing, false); + source_node.param("marking", marking, true); + + if (!sensor_frame.empty()) + { + sensor_frame = tf::resolve(tf_prefix, sensor_frame); + } + + if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) + { + ROS_FATAL("Only topics that use point clouds or laser scans are currently supported"); + throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); + } + + std::string raytrace_range_param_name, obstacle_range_param_name; + + // get the obstacle range for the sensor + double obstacle_range = 2.5; + if (source_node.searchParam("obstacle_range", obstacle_range_param_name)) + { + source_node.getParam(obstacle_range_param_name, obstacle_range); + } + + // get the raytrace range for the sensor + double raytrace_range = 3.0; + if (source_node.searchParam("raytrace_range", raytrace_range_param_name)) + { + source_node.getParam(raytrace_range_param_name, raytrace_range); + } + + ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), + sensor_frame.c_str()); + + // create an observation buffer + observation_buffers_.push_back( + boost::shared_ptr < ObservationBuffer + > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, + max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, + sensor_frame, transform_tolerance))); + + // check if we'll add this buffer to our marking observation buffers + if (marking) + marking_buffers_.push_back(observation_buffers_.back()); + + // check if we'll also add this buffer to our clearing observation buffers + if (clearing) + clearing_buffers_.push_back(observation_buffers_.back()); + + ROS_DEBUG( + "Created an observation buffer for source %s, topic %s, global frame: %s, " + "expected update rate: %.2f, observation persistence: %.2f", + source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); + + // create a callback for the topic + if (data_type == "LaserScan") + { + boost::shared_ptr < message_filters::Subscriber + > sub(new message_filters::Subscriber(g_nh, topic, 50)); + + boost::shared_ptr < tf::MessageFilter + > filter(new tf::MessageFilter(*sub, *tf_, global_frame_, 50)); + + if (inf_is_valid) + { + filter->registerCallback( + boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1, observation_buffers_.back())); + } + else + { + filter->registerCallback( + boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back())); + } + + observation_subscribers_.push_back(sub); + observation_notifiers_.push_back(filter); + + observation_notifiers_.back()->setTolerance(ros::Duration(0.05)); + } + else if (data_type == "PointCloud") + { + boost::shared_ptr < message_filters::Subscriber + > sub(new message_filters::Subscriber(g_nh, topic, 50)); + + if (inf_is_valid) + { + ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); + } + + boost::shared_ptr < tf::MessageFilter + > filter(new tf::MessageFilter(*sub, *tf_, global_frame_, 50)); + filter->registerCallback( + boost::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back())); + + observation_subscribers_.push_back(sub); + observation_notifiers_.push_back(filter); + } + else + { + boost::shared_ptr < message_filters::Subscriber + > sub(new message_filters::Subscriber(g_nh, topic, 50)); + + if (inf_is_valid) + { + ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); + } + + boost::shared_ptr < tf::MessageFilter + > filter(new tf::MessageFilter(*sub, *tf_, global_frame_, 50)); + filter->registerCallback( + boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back())); + + observation_subscribers_.push_back(sub); + observation_notifiers_.push_back(filter); + } + + if (sensor_frame != "") + { + std::vector < std::string > target_frames; + target_frames.push_back(global_frame_); + target_frames.push_back(sensor_frame); + observation_notifiers_.back()->setTargetFrames(target_frames); + } + } + + dsrv_ = NULL; + setupDynamicReconfigure(nh); +} + +void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh) +{ + dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = boost::bind( + &ObstacleLayer::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); +} + +ObstacleLayer::~ObstacleLayer() +{ + if (dsrv_) + delete dsrv_; +} +void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level) +{ + enabled_ = config.enabled; + footprint_clearing_enabled_ = config.footprint_clearing_enabled; + max_obstacle_height_ = config.max_obstacle_height; + combination_method_ = config.combination_method; +} + +void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, + const boost::shared_ptr& buffer) +{ + // project the laser into a point cloud + sensor_msgs::PointCloud2 cloud; + cloud.header = message->header; + + // project the scan into a point cloud + try + { + projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_); + } + catch (tf::TransformException &ex) + { + ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), + ex.what()); + projector_.projectLaser(*message, cloud); + } + + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(cloud); + buffer->unlock(); +} + +void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message, + const boost::shared_ptr& buffer) +{ + // Filter positive infinities ("Inf"s) to max_range. + float epsilon = 0.0001; // a tenth of a millimeter + sensor_msgs::LaserScan message = *raw_message; + for (size_t i = 0; i < message.ranges.size(); i++) + { + float range = message.ranges[ i ]; + if (!std::isfinite(range) && range > 0) + { + message.ranges[ i ] = message.range_max - epsilon; + } + } + + // project the laser into a point cloud + sensor_msgs::PointCloud2 cloud; + cloud.header = message.header; + + // project the scan into a point cloud + try + { + projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); + } + catch (tf::TransformException &ex) + { + ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", + global_frame_.c_str(), ex.what()); + projector_.projectLaser(message, cloud); + } + + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(cloud); + buffer->unlock(); +} + +void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, + const boost::shared_ptr& buffer) +{ + sensor_msgs::PointCloud2 cloud2; + + if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2)) + { + ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message"); + return; + } + + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(cloud2); + buffer->unlock(); +} + +void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, + const boost::shared_ptr& buffer) +{ + // buffer the point cloud + buffer->lock(); + buffer->bufferCloud(*message); + buffer->unlock(); +} + +void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, + double* min_y, double* max_x, double* max_y) +{ + if (rolling_window_) + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + if (!enabled_) + return; + useExtraBounds(min_x, min_y, max_x, max_y); + + bool current = true; + std::vector observations, clearing_observations; + + // get the marking observations + current = current && getMarkingObservations(observations); + + // get the clearing observations + current = current && getClearingObservations(clearing_observations); + + // update the global current status + current_ = current; + + // raytrace freespace + for (unsigned int i = 0; i < clearing_observations.size(); ++i) + { + raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); + } + + // place the new obstacles into a priority queue... each with a priority of zero to begin with + for (std::vector::const_iterator it = observations.begin(); it != observations.end(); ++it) + { + const Observation& obs = *it; + + const pcl::PointCloud& cloud = *(obs.cloud_); + + double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; + + for (unsigned int i = 0; i < cloud.points.size(); ++i) + { + double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z; + + // if the obstacle is too high or too far away from the robot we won't add it + if (pz > max_obstacle_height_) + { + ROS_DEBUG("The point is too high"); + continue; + } + + // compute the squared distance from the hitpoint to the pointcloud's origin + double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + + (pz - obs.origin_.z) * (pz - obs.origin_.z); + + // if the point is far enough away... we won't consider it + if (sq_dist >= sq_obstacle_range) + { + ROS_DEBUG("The point is too far away"); + continue; + } + + // now we need to compute the map coordinates for the observation + unsigned int mx, my; + if (!worldToMap(px, py, mx, my)) + { + ROS_DEBUG("Computing map coords failed"); + continue; + } + + unsigned int index = getIndex(mx, my); + costmap_[index] = LETHAL_OBSTACLE; + touch(px, py, min_x, min_y, max_x, max_y); + } + } + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y) +{ + if (!footprint_clearing_enabled_) return; + transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + for (unsigned int i = 0; i < transformed_footprint_.size(); i++) + { + touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); + } +} + +void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) + return; + + if (footprint_clearing_enabled_) + { + setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); + } + + switch (combination_method_) + { + case 0: // Overwrite + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + case 1: // Maximum + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Nothing + break; + } +} + +void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing) +{ + if (marking) + static_marking_observations_.push_back(obs); + if (clearing) + static_clearing_observations_.push_back(obs); +} + +void ObstacleLayer::clearStaticObservations(bool marking, bool clearing) +{ + if (marking) + static_marking_observations_.clear(); + if (clearing) + static_clearing_observations_.clear(); +} + +bool ObstacleLayer::getMarkingObservations(std::vector& marking_observations) const +{ + bool current = true; + // get the marking observations + for (unsigned int i = 0; i < marking_buffers_.size(); ++i) + { + marking_buffers_[i]->lock(); + marking_buffers_[i]->getObservations(marking_observations); + current = marking_buffers_[i]->isCurrent() && current; + marking_buffers_[i]->unlock(); + } + marking_observations.insert(marking_observations.end(), + static_marking_observations_.begin(), static_marking_observations_.end()); + return current; +} + +bool ObstacleLayer::getClearingObservations(std::vector& clearing_observations) const +{ + bool current = true; + // get the clearing observations + for (unsigned int i = 0; i < clearing_buffers_.size(); ++i) + { + clearing_buffers_[i]->lock(); + clearing_buffers_[i]->getObservations(clearing_observations); + current = clearing_buffers_[i]->isCurrent() && current; + clearing_buffers_[i]->unlock(); + } + clearing_observations.insert(clearing_observations.end(), + static_clearing_observations_.begin(), static_clearing_observations_.end()); + return current; +} + +void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, + double* max_x, double* max_y) +{ + double ox = clearing_observation.origin_.x; + double oy = clearing_observation.origin_.y; + pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_); + + // get the map coordinates of the origin of the sensor + unsigned int x0, y0; + if (!worldToMap(ox, oy, x0, y0)) + { + ROS_WARN_THROTTLE( + 1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", + ox, oy); + return; + } + + // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later + double origin_x = origin_x_, origin_y = origin_y_; + double map_end_x = origin_x + size_x_ * resolution_; + double map_end_y = origin_y + size_y_ * resolution_; + + + touch(ox, oy, min_x, min_y, max_x, max_y); + + // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it + for (unsigned int i = 0; i < cloud.points.size(); ++i) + { + double wx = cloud.points[i].x; + double wy = cloud.points[i].y; + + // now we also need to make sure that the enpoint we're raytracing + // to isn't off the costmap and scale if necessary + double a = wx - ox; + double b = wy - oy; + + // the minimum value to raytrace from is the origin + if (wx < origin_x) + { + double t = (origin_x - ox) / a; + wx = origin_x; + wy = oy + b * t; + } + if (wy < origin_y) + { + double t = (origin_y - oy) / b; + wx = ox + a * t; + wy = origin_y; + } + + // the maximum value to raytrace to is the end of the map + if (wx > map_end_x) + { + double t = (map_end_x - ox) / a; + wx = map_end_x - .001; + wy = oy + b * t; + } + if (wy > map_end_y) + { + double t = (map_end_y - oy) / b; + wx = ox + a * t; + wy = map_end_y - .001; + } + + // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint + unsigned int x1, y1; + + // check for legality just in case + if (!worldToMap(wx, wy, x1, y1)) + continue; + + unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); + MarkCell marker(costmap_, FREE_SPACE); + // and finally... we can execute our trace to clear obstacles along that line + raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); + + updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); + } +} + +void ObstacleLayer::activate() +{ + // if we're stopped we need to re-subscribe to topics + for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) + { + if (observation_subscribers_[i] != NULL) + observation_subscribers_[i]->subscribe(); + } + + for (unsigned int i = 0; i < observation_buffers_.size(); ++i) + { + if (observation_buffers_[i]) + observation_buffers_[i]->resetLastUpdated(); + } +} +void ObstacleLayer::deactivate() +{ + for (unsigned int i = 0; i < observation_subscribers_.size(); ++i) + { + if (observation_subscribers_[i] != NULL) + observation_subscribers_[i]->unsubscribe(); + } +} + +void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, + double* min_x, double* min_y, double* max_x, double* max_y) +{ + double dx = wx-ox, dy = wy-oy; + double full_distance = hypot(dx, dy); + double scale = std::min(1.0, range / full_distance); + double ex = ox + dx * scale, ey = oy + dy * scale; + touch(ex, ey, min_x, min_y, max_x, max_y); +} + +void ObstacleLayer::reset() +{ + deactivate(); + resetMaps(); + current_ = true; + activate(); +} + +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/plugins/static_layer.cpp b/src/libs/nav2_util/src/costmap_2d/plugins/static_layer.cpp new file mode 100644 index 0000000000..709b8bbce4 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/plugins/static_layer.cpp @@ -0,0 +1,346 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * Copyright (c) 2015, Fetch Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer) + +using costmap_2d::NO_INFORMATION; +using costmap_2d::LETHAL_OBSTACLE; +using costmap_2d::FREE_SPACE; + +namespace costmap_2d +{ + +StaticLayer::StaticLayer() : dsrv_(NULL) {} + +StaticLayer::~StaticLayer() +{ + if (dsrv_) + delete dsrv_; +} + +void StaticLayer::onInitialize() +{ + ros::NodeHandle nh("~/" + name_), g_nh; + current_ = true; + + global_frame_ = layered_costmap_->getGlobalFrameID(); + + std::string map_topic; + nh.param("map_topic", map_topic, std::string("map")); + nh.param("first_map_only", first_map_only_, false); + nh.param("subscribe_to_updates", subscribe_to_updates_, false); + + nh.param("track_unknown_space", track_unknown_space_, true); + nh.param("use_maximum", use_maximum_, false); + + int temp_lethal_threshold, temp_unknown_cost_value; + nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100)); + nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1)); + nh.param("trinary_costmap", trinary_costmap_, true); + + lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); + unknown_cost_value_ = temp_unknown_cost_value; + + // Only resubscribe if topic has changed + if (map_sub_.getTopic() != ros::names::resolve(map_topic)) + { + // we'll subscribe to the latched topic that the map server uses + ROS_INFO("Requesting the map..."); + map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this); + map_received_ = false; + has_updated_data_ = false; + + ros::Rate r(10); + while (!map_received_ && g_nh.ok()) + { + ros::spinOnce(); + r.sleep(); + } + + ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution()); + + if (subscribe_to_updates_) + { + ROS_INFO("Subscribing to updates"); + map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this); + + } + } + else + { + has_updated_data_ = true; + } + + if (dsrv_) + { + delete dsrv_; + } + + dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = boost::bind( + &StaticLayer::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); +} + +void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level) +{ + if (config.enabled != enabled_) + { + enabled_ = config.enabled; + has_updated_data_ = true; + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + } +} + +void StaticLayer::matchSize() +{ + // If we are using rolling costmap, the static map size is + // unrelated to the size of the layered costmap + if (!layered_costmap_->isRolling()) + { + Costmap2D* master = layered_costmap_->getCostmap(); + resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), + master->getOriginX(), master->getOriginY()); + } +} + +unsigned char StaticLayer::interpretValue(unsigned char value) +{ + // check if the static value is above the unknown or lethal thresholds + if (track_unknown_space_ && value == unknown_cost_value_) + return NO_INFORMATION; + else if (!track_unknown_space_ && value == unknown_cost_value_) + return FREE_SPACE; + else if (value >= lethal_threshold_) + return LETHAL_OBSTACLE; + else if (trinary_costmap_) + return FREE_SPACE; + + double scale = (double) value / lethal_threshold_; + return scale * LETHAL_OBSTACLE; +} + +void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map) +{ + unsigned int size_x = new_map->info.width, size_y = new_map->info.height; + + ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution); + + // resize costmap if size, resolution or origin do not match + Costmap2D* master = layered_costmap_->getCostmap(); + if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || + master->getSizeInCellsY() != size_y || + master->getResolution() != new_map->info.resolution || + master->getOriginX() != new_map->info.origin.position.x || + master->getOriginY() != new_map->info.origin.position.y || + !layered_costmap_->isSizeLocked())) + { + // Update the size of the layered costmap (and all layers, including this one) + ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); + layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x, + new_map->info.origin.position.y, true); + } + else if (size_x_ != size_x || size_y_ != size_y || + resolution_ != new_map->info.resolution || + origin_x_ != new_map->info.origin.position.x || + origin_y_ != new_map->info.origin.position.y) + { + // only update the size of the costmap stored locally in this layer + ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); + resizeMap(size_x, size_y, new_map->info.resolution, + new_map->info.origin.position.x, new_map->info.origin.position.y); + } + + unsigned int index = 0; + + // initialize the costmap with static data + for (unsigned int i = 0; i < size_y; ++i) + { + for (unsigned int j = 0; j < size_x; ++j) + { + unsigned char value = new_map->data[index]; + costmap_[index] = interpretValue(value); + ++index; + } + } + map_frame_ = new_map->header.frame_id; + + // we have a new map, update full size of map + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + map_received_ = true; + has_updated_data_ = true; + + // shutdown the map subscrber if firt_map_only_ flag is on + if (first_map_only_) + { + ROS_INFO("Shutting down the map subscriber. first_map_only flag is on"); + map_sub_.shutdown(); + } +} + +void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update) +{ + unsigned int di = 0; + for (unsigned int y = 0; y < update->height ; y++) + { + unsigned int index_base = (update->y + y) * size_x_; + for (unsigned int x = 0; x < update->width ; x++) + { + unsigned int index = index_base + x + update->x; + costmap_[index] = interpretValue(update->data[di++]); + } + } + x_ = update->x; + y_ = update->y; + width_ = update->width; + height_ = update->height; + has_updated_data_ = true; +} + +void StaticLayer::activate() +{ + onInitialize(); +} + +void StaticLayer::deactivate() +{ + map_sub_.shutdown(); + if (subscribe_to_updates_) + map_update_sub_.shutdown(); +} + +void StaticLayer::reset() +{ + if (first_map_only_) + { + has_updated_data_ = true; + } + else + { + onInitialize(); + } +} + +void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y) +{ + + if( !layered_costmap_->isRolling() ){ + if (!map_received_ || !(has_updated_data_ || has_extra_bounds_)) + return; + } + + useExtraBounds(min_x, min_y, max_x, max_y); + + double wx, wy; + + mapToWorld(x_, y_, wx, wy); + *min_x = std::min(wx, *min_x); + *min_y = std::min(wy, *min_y); + + mapToWorld(x_ + width_, y_ + height_, wx, wy); + *max_x = std::max(wx, *max_x); + *max_y = std::max(wy, *max_y); + + has_updated_data_ = false; +} + +void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + if (!map_received_) + return; + + if (!enabled_) + return; + + if (!layered_costmap_->isRolling()) + { + // if not rolling, the layered costmap (master_grid) has same coordinates as this layer + if (!use_maximum_) + updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j); + else + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + } + else + { + // If rolling window, the master_grid is unlikely to have same coordinates as this layer + unsigned int mx, my; + double wx, wy; + // Might even be in a different frame + tf::StampedTransform transform; + try + { + tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform); + } + catch (tf::TransformException ex) + { + ROS_ERROR("%s", ex.what()); + return; + } + // Copy map data given proper transformations + for (unsigned int i = min_i; i < max_i; ++i) + { + for (unsigned int j = min_j; j < max_j; ++j) + { + // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates + layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy); + // Transform from global_frame_ to map_frame_ + tf::Point p(wx, wy, 0); + p = transform(p); + // Set master_grid with cell from map + if (worldToMap(p.x(), p.y(), mx, my)) + { + if (!use_maximum_) + master_grid.setCost(i, j, getCost(mx, my)); + else + master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j))); + } + } + } + } +} + +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/plugins/voxel_layer.cpp b/src/libs/nav2_util/src/costmap_2d/plugins/voxel_layer.cpp new file mode 100644 index 0000000000..7915eb8baf --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/plugins/voxel_layer.cpp @@ -0,0 +1,442 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include +#include + +#define VOXEL_BITS 16 +PLUGINLIB_EXPORT_CLASS(costmap_2d::VoxelLayer, costmap_2d::Layer) + +using costmap_2d::NO_INFORMATION; +using costmap_2d::LETHAL_OBSTACLE; +using costmap_2d::FREE_SPACE; + +using costmap_2d::ObservationBuffer; +using costmap_2d::Observation; + +namespace costmap_2d +{ + +void VoxelLayer::onInitialize() +{ + ObstacleLayer::onInitialize(); + ros::NodeHandle private_nh("~/" + name_); + + private_nh.param("publish_voxel_map", publish_voxel_, false); + if (publish_voxel_) + voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1); + + clearing_endpoints_pub_ = private_nh.advertise("clearing_endpoints", 1); +} + +void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh) +{ + voxel_dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = boost::bind( + &VoxelLayer::reconfigureCB, this, _1, _2); + voxel_dsrv_->setCallback(cb); +} + +VoxelLayer::~VoxelLayer() +{ + if (voxel_dsrv_) + delete voxel_dsrv_; +} + +void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level) +{ + enabled_ = config.enabled; + footprint_clearing_enabled_ = config.footprint_clearing_enabled; + max_obstacle_height_ = config.max_obstacle_height; + size_z_ = config.z_voxels; + origin_z_ = config.origin_z; + z_resolution_ = config.z_resolution; + unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_); + mark_threshold_ = config.mark_threshold; + combination_method_ = config.combination_method; + matchSize(); +} + +void VoxelLayer::matchSize() +{ + ObstacleLayer::matchSize(); + voxel_grid_.resize(size_x_, size_y_, size_z_); + ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); +} + +void VoxelLayer::reset() +{ + deactivate(); + resetMaps(); + voxel_grid_.reset(); + activate(); +} + +void VoxelLayer::resetMaps() +{ + Costmap2D::resetMaps(); + voxel_grid_.reset(); +} + +void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, + double* min_y, double* max_x, double* max_y) +{ + if (rolling_window_) + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + if (!enabled_) + return; + useExtraBounds(min_x, min_y, max_x, max_y); + + bool current = true; + std::vector observations, clearing_observations; + + // get the marking observations + current = getMarkingObservations(observations) && current; + + // get the clearing observations + current = getClearingObservations(clearing_observations) && current; + + // update the global current status + current_ = current; + + // raytrace freespace + for (unsigned int i = 0; i < clearing_observations.size(); ++i) + { + raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); + } + + // place the new obstacles into a priority queue... each with a priority of zero to begin with + for (std::vector::const_iterator it = observations.begin(); it != observations.end(); ++it) + { + const Observation& obs = *it; + + const pcl::PointCloud& cloud = *(obs.cloud_); + + double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; + + for (unsigned int i = 0; i < cloud.points.size(); ++i) + { + // if the obstacle is too high or too far away from the robot we won't add it + if (cloud.points[i].z > max_obstacle_height_) + continue; + + // compute the squared distance from the hitpoint to the pointcloud's origin + double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x) + + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y) + + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z); + + // if the point is far enough away... we won't consider it + if (sq_dist >= sq_obstacle_range) + continue; + + // now we need to compute the map coordinates for the observation + unsigned int mx, my, mz; + if (cloud.points[i].z < origin_z_) + { + if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz)) + continue; + } + else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz)) + { + continue; + } + + // mark the cell in the voxel grid and check if we should also mark it in the costmap + if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) + { + unsigned int index = getIndex(mx, my); + + costmap_[index] = LETHAL_OBSTACLE; + touch((double)cloud.points[i].x, (double)cloud.points[i].y, min_x, min_y, max_x, max_y); + } + } + } + + if (publish_voxel_) + { + costmap_2d::VoxelGrid grid_msg; + unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); + grid_msg.size_x = voxel_grid_.sizeX(); + grid_msg.size_y = voxel_grid_.sizeY(); + grid_msg.size_z = voxel_grid_.sizeZ(); + grid_msg.data.resize(size); + memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); + + grid_msg.origin.x = origin_x_; + grid_msg.origin.y = origin_y_; + grid_msg.origin.z = origin_z_; + + grid_msg.resolutions.x = resolution_; + grid_msg.resolutions.y = resolution_; + grid_msg.resolutions.z = z_resolution_; + grid_msg.header.frame_id = global_frame_; + grid_msg.header.stamp = ros::Time::now(); + voxel_pub_.publish(grid_msg); + } + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info) +{ + // get the cell coordinates of the center point of the window + unsigned int mx, my; + if (!worldToMap(wx, wy, mx, my)) + return; + + // compute the bounds of the window + double start_x = wx - w_size_x / 2; + double start_y = wy - w_size_y / 2; + double end_x = start_x + w_size_x; + double end_y = start_y + w_size_y; + + // scale the window based on the bounds of the costmap + start_x = std::max(origin_x_, start_x); + start_y = std::max(origin_y_, start_y); + + end_x = std::min(origin_x_ + getSizeInMetersX(), end_x); + end_y = std::min(origin_y_ + getSizeInMetersY(), end_y); + + // get the map coordinates of the bounds of the window + unsigned int map_sx, map_sy, map_ex, map_ey; + + // check for legality just in case + if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey)) + return; + + // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation + unsigned int index = getIndex(map_sx, map_sy); + unsigned char* current = &costmap_[index]; + for (unsigned int j = map_sy; j <= map_ey; ++j) + { + for (unsigned int i = map_sx; i <= map_ex; ++i) + { + // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it + if (*current != LETHAL_OBSTACLE) + { + if (clear_no_info || *current != NO_INFORMATION) + { + *current = FREE_SPACE; + voxel_grid_.clearVoxelColumn(index); + } + } + current++; + index++; + } + current += size_x_ - (map_ex - map_sx) - 1; + index += size_x_ - (map_ex - map_sx) - 1; + } +} + +void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, + double* max_x, double* max_y) +{ + if (clearing_observation.cloud_->points.size() == 0) + return; + + double sensor_x, sensor_y, sensor_z; + double ox = clearing_observation.origin_.x; + double oy = clearing_observation.origin_.y; + double oz = clearing_observation.origin_.z; + + if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) + { + ROS_WARN_THROTTLE( + 1.0, + "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", + ox, oy, oz); + return; + } + + bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0); + if (publish_clearing_points) + { + clearing_endpoints_.points.clear(); + clearing_endpoints_.points.reserve(clearing_observation.cloud_->points.size()); + } + + // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later + double map_end_x = origin_x_ + getSizeInMetersX(); + double map_end_y = origin_y_ + getSizeInMetersY(); + + for (unsigned int i = 0; i < clearing_observation.cloud_->points.size(); ++i) + { + double wpx = clearing_observation.cloud_->points[i].x; + double wpy = clearing_observation.cloud_->points[i].y; + double wpz = clearing_observation.cloud_->points[i].z; + + double distance = dist(ox, oy, oz, wpx, wpy, wpz); + double scaling_fact = 1.0; + scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0); + wpx = scaling_fact * (wpx - ox) + ox; + wpy = scaling_fact * (wpy - oy) + oy; + wpz = scaling_fact * (wpz - oz) + oz; + + double a = wpx - ox; + double b = wpy - oy; + double c = wpz - oz; + double t = 1.0; + + // we can only raytrace to a maximum z height + if (wpz > max_obstacle_height_) + { + // we know we want the vector's z value to be max_z + t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c)); + } + // and we can only raytrace down to the floor + else if (wpz < origin_z_) + { + // we know we want the vector's z value to be 0.0 + t = std::min(t, (origin_z_ - oz) / c); + } + + // the minimum value to raytrace from is the origin + if (wpx < origin_x_) + { + t = std::min(t, (origin_x_ - ox) / a); + } + if (wpy < origin_y_) + { + t = std::min(t, (origin_y_ - oy) / b); + } + + // the maximum value to raytrace to is the end of the map + if (wpx > map_end_x) + { + t = std::min(t, (map_end_x - ox) / a); + } + if (wpy > map_end_y) + { + t = std::min(t, (map_end_y - oy) / b); + } + + wpx = ox + a * t; + wpy = oy + b * t; + wpz = oz + c * t; + + double point_x, point_y, point_z; + if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) + { + unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); + + // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); + voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, + unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, + cell_raytrace_range); + + updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); + + if (publish_clearing_points) + { + geometry_msgs::Point32 point; + point.x = wpx; + point.y = wpy; + point.z = wpz; + clearing_endpoints_.points.push_back(point); + } + } + } + + if (publish_clearing_points) + { + clearing_endpoints_.header.frame_id = global_frame_; + clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header).stamp; + clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq; + + clearing_endpoints_pub_.publish(clearing_endpoints_); + } +} + +void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) +{ + // project the new origin into the grid + int cell_ox, cell_oy; + cell_ox = int((new_origin_x - origin_x_) / resolution_); + cell_oy = int((new_origin_y - origin_y_) / resolution_); + + // compute the associated world coordinates for the origin cell + // beacuase we want to keep things grid-aligned + double new_grid_ox, new_grid_oy; + new_grid_ox = origin_x_ + cell_ox * resolution_; + new_grid_oy = origin_y_ + cell_oy * resolution_; + + // To save casting from unsigned int to int a bunch of times + int size_x = size_x_; + int size_y = size_y_; + + // we need to compute the overlap of the new and existing windows + int lower_left_x, lower_left_y, upper_right_x, upper_right_y; + lower_left_x = std::min(std::max(cell_ox, 0), size_x); + lower_left_y = std::min(std::max(cell_oy, 0), size_y); + upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x); + upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y); + + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + + // we need a map to store the obstacles in the window temporarily + unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; + unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y]; + unsigned int* voxel_map = voxel_grid_.getData(); + + // copy the local window in the costmap to the local map + copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); + copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x, + cell_size_y); + + // we'll reset our maps to unknown space if appropriate + resetMaps(); + + // update the origin with the appropriate world coordinates + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; + + // compute the starting cell location for copying data back in + int start_x = lower_left_x - cell_ox; + int start_y = lower_left_y - cell_oy; + + // now we want to copy the overlapping information back into the map, but in its new location + copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); + copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y); + + // make sure to clean up + delete[] local_map; + delete[] local_voxel_map; +} + +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/array_parser.cpp b/src/libs/nav2_util/src/costmap_2d/src/array_parser.cpp new file mode 100644 index 0000000000..144a860fc6 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/array_parser.cpp @@ -0,0 +1,115 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * author: Dave Hershberger + */ + +#include // for EOF +#include +#include +#include + +namespace costmap_2d +{ + +/** @brief Parse a vector of vector of floats from a string. + * @param input + * @param error_return + * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */ +std::vector > parseVVF(const std::string& input, std::string& error_return) +{ + std::vector > result; + + std::stringstream input_ss(input); + int depth = 0; + std::vector current_vector; + while (!!input_ss && !input_ss.eof()) + { + switch (input_ss.peek()) + { + case EOF: + break; + case '[': + depth++; + if (depth > 2) + { + error_return = "Array depth greater than 2"; + return result; + } + input_ss.get(); + current_vector.clear(); + break; + case ']': + depth--; + if (depth < 0) + { + error_return = "More close ] than open ["; + return result; + } + input_ss.get(); + if (depth == 1) + { + result.push_back(current_vector); + } + break; + case ',': + case ' ': + case '\t': + input_ss.get(); + break; + default: // All other characters should be part of the numbers. + if (depth != 2) + { + std::stringstream err_ss; + err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'."; + error_return = err_ss.str(); + return result; + } + float value; + input_ss >> value; + if (!!input_ss) + { + current_vector.push_back(value); + } + break; + } + } + + if (depth != 0) + { + error_return = "Unterminated vector string."; + } + else + { + error_return = ""; + } + + return result; +} + +} // end namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_2d.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d.cpp new file mode 100644 index 0000000000..dfc0358173 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d.cpp @@ -0,0 +1,484 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include + +using namespace std; + +namespace costmap_2d +{ +Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, + double origin_x, double origin_y, unsigned char default_value) : + size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), + origin_y_(origin_y), costmap_(NULL), default_value_(default_value) +{ + access_ = new mutex_t(); + + // create the costmap + initMaps(size_x_, size_y_); + resetMaps(); +} + +void Costmap2D::deleteMaps() +{ + // clean up data + boost::unique_lock lock(*access_); + delete[] costmap_; + costmap_ = NULL; +} + +void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y) +{ + boost::unique_lock lock(*access_); + delete[] costmap_; + costmap_ = new unsigned char[size_x * size_y]; +} + +void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, + double origin_x, double origin_y) +{ + size_x_ = size_x; + size_y_ = size_y; + resolution_ = resolution; + origin_x_ = origin_x; + origin_y_ = origin_y; + + initMaps(size_x, size_y); + + // reset our maps to have no information + resetMaps(); +} + +void Costmap2D::resetMaps() +{ + boost::unique_lock lock(*access_); + memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char)); +} + +void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn) +{ + boost::unique_lock lock(*(access_)); + unsigned int len = xn - x0; + for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) + memset(costmap_ + y, default_value_, len * sizeof(unsigned char)); +} + +bool Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, + double win_size_y) +{ + // check for self windowing + if (this == &map) + { + // ROS_ERROR("Cannot convert this costmap into a window of itself"); + return false; + } + + // clean up old data + deleteMaps(); + + // compute the bounds of our new map + unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y; + if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y) + || !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y)) + { + // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of"); + return false; + } + + size_x_ = upper_right_x - lower_left_x; + size_y_ = upper_right_y - lower_left_y; + resolution_ = map.resolution_; + origin_x_ = win_origin_x; + origin_y_ = win_origin_y; + + // initialize our various maps and reset markers for inflation + initMaps(size_x_, size_y_); + + // copy the window of the static map and the costmap that we're taking + copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_); + return true; +} + +Costmap2D& Costmap2D::operator=(const Costmap2D& map) +{ + // check for self assignement + if (this == &map) + return *this; + + // clean up old data + deleteMaps(); + + size_x_ = map.size_x_; + size_y_ = map.size_y_; + resolution_ = map.resolution_; + origin_x_ = map.origin_x_; + origin_y_ = map.origin_y_; + + // initialize our various maps + initMaps(size_x_, size_y_); + + // copy the cost map + memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); + + return *this; +} + +Costmap2D::Costmap2D(const Costmap2D& map) : + costmap_(NULL) +{ + access_ = new mutex_t(); + *this = map; +} + +// just initialize everything to NULL by default +Costmap2D::Costmap2D() : + size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL) +{ + access_ = new mutex_t(); +} + +Costmap2D::~Costmap2D() +{ + deleteMaps(); + delete access_; +} + +unsigned int Costmap2D::cellDistance(double world_dist) +{ + double cells_dist = max(0.0, ceil(world_dist / resolution_)); + return (unsigned int)cells_dist; +} + +unsigned char* Costmap2D::getCharMap() const +{ + return costmap_; +} + +unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const +{ + return costmap_[getIndex(mx, my)]; +} + +void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) +{ + costmap_[getIndex(mx, my)] = cost; +} + +void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const +{ + wx = origin_x_ + (mx + 0.5) * resolution_; + wy = origin_y_ + (my + 0.5) * resolution_; +} + +bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const +{ + if (wx < origin_x_ || wy < origin_y_) + return false; + + mx = (int)((wx - origin_x_) / resolution_); + my = (int)((wy - origin_y_) / resolution_); + + if (mx < size_x_ && my < size_y_) + return true; + + return false; +} + +void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const +{ + mx = (int)((wx - origin_x_) / resolution_); + my = (int)((wy - origin_y_) / resolution_); +} + +void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const +{ + // Here we avoid doing any math to wx,wy before comparing them to + // the bounds, so their values can go out to the max and min values + // of double floating point. + if (wx < origin_x_) + { + mx = 0; + } + else if (wx > resolution_ * size_x_ + origin_x_) + { + mx = size_x_ - 1; + } + else + { + mx = (int)((wx - origin_x_) / resolution_); + } + + if (wy < origin_y_) + { + my = 0; + } + else if (wy > resolution_ * size_y_ + origin_y_) + { + my = size_y_ - 1; + } + else + { + my = (int)((wy - origin_y_) / resolution_); + } +} + +void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) +{ + // project the new origin into the grid + int cell_ox, cell_oy; + cell_ox = int((new_origin_x - origin_x_) / resolution_); + cell_oy = int((new_origin_y - origin_y_) / resolution_); + + // compute the associated world coordinates for the origin cell + // because we want to keep things grid-aligned + double new_grid_ox, new_grid_oy; + new_grid_ox = origin_x_ + cell_ox * resolution_; + new_grid_oy = origin_y_ + cell_oy * resolution_; + + // To save casting from unsigned int to int a bunch of times + int size_x = size_x_; + int size_y = size_y_; + + // we need to compute the overlap of the new and existing windows + int lower_left_x, lower_left_y, upper_right_x, upper_right_y; + lower_left_x = min(max(cell_ox, 0), size_x); + lower_left_y = min(max(cell_oy, 0), size_y); + upper_right_x = min(max(cell_ox + size_x, 0), size_x); + upper_right_y = min(max(cell_oy + size_y, 0), size_y); + + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + + // we need a map to store the obstacles in the window temporarily + unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; + + // copy the local window in the costmap to the local map + copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); + + // now we'll set the costmap to be completely unknown if we track unknown space + resetMaps(); + + // update the origin with the appropriate world coordinates + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; + + // compute the starting cell location for copying data back in + int start_x = lower_left_x - cell_ox; + int start_y = lower_left_y - cell_oy; + + // now we want to copy the overlapping information back into the map, but in its new location + copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); + + // make sure to clean up + delete[] local_map; +} + +bool Costmap2D::setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value) +{ + // we assume the polygon is given in the global_frame... we need to transform it to map coordinates + std::vector map_polygon; + for (unsigned int i = 0; i < polygon.size(); ++i) + { + MapLocation loc; + if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) + { + // ("Polygon lies outside map bounds, so we can't fill it"); + return false; + } + map_polygon.push_back(loc); + } + + std::vector polygon_cells; + + // get the cells that fill the polygon + convexFillCells(map_polygon, polygon_cells); + + // set the cost of those cells + for (unsigned int i = 0; i < polygon_cells.size(); ++i) + { + unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y); + costmap_[index] = cost_value; + } + return true; +} + +void Costmap2D::polygonOutlineCells(const std::vector& polygon, std::vector& polygon_cells) +{ + PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); + for (unsigned int i = 0; i < polygon.size() - 1; ++i) + { + raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); + } + if (!polygon.empty()) + { + unsigned int last_index = polygon.size() - 1; + // we also need to close the polygon by going from the last point to the first + raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y); + } +} + +void Costmap2D::convexFillCells(const std::vector& polygon, std::vector& polygon_cells) +{ + // we need a minimum polygon of a triangle + if (polygon.size() < 3) + return; + + // first get the cells that make up the outline of the polygon + polygonOutlineCells(polygon, polygon_cells); + + // quick bubble sort to sort points by x + MapLocation swap; + unsigned int i = 0; + while (i < polygon_cells.size() - 1) + { + if (polygon_cells[i].x > polygon_cells[i + 1].x) + { + swap = polygon_cells[i]; + polygon_cells[i] = polygon_cells[i + 1]; + polygon_cells[i + 1] = swap; + + if (i > 0) + --i; + } + else + ++i; + } + + i = 0; + MapLocation min_pt; + MapLocation max_pt; + unsigned int min_x = polygon_cells[0].x; + unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x; + + // walk through each column and mark cells inside the polygon + for (unsigned int x = min_x; x <= max_x; ++x) + { + if (i >= polygon_cells.size() - 1) + break; + + if (polygon_cells[i].y < polygon_cells[i + 1].y) + { + min_pt = polygon_cells[i]; + max_pt = polygon_cells[i + 1]; + } + else + { + min_pt = polygon_cells[i + 1]; + max_pt = polygon_cells[i]; + } + + i += 2; + while (i < polygon_cells.size() && polygon_cells[i].x == x) + { + if (polygon_cells[i].y < min_pt.y) + min_pt = polygon_cells[i]; + else if (polygon_cells[i].y > max_pt.y) + max_pt = polygon_cells[i]; + ++i; + } + + MapLocation pt; + // loop though cells in the column + for (unsigned int y = min_pt.y; y < max_pt.y; ++y) + { + pt.x = x; + pt.y = y; + polygon_cells.push_back(pt); + } + } +} + +unsigned int Costmap2D::getSizeInCellsX() const +{ + return size_x_; +} + +unsigned int Costmap2D::getSizeInCellsY() const +{ + return size_y_; +} + +double Costmap2D::getSizeInMetersX() const +{ + return (size_x_ - 1 + 0.5) * resolution_; +} + +double Costmap2D::getSizeInMetersY() const +{ + return (size_y_ - 1 + 0.5) * resolution_; +} + +double Costmap2D::getOriginX() const +{ + return origin_x_; +} + +double Costmap2D::getOriginY() const +{ + return origin_y_; +} + +double Costmap2D::getResolution() const +{ + return resolution_; +} + +bool Costmap2D::saveMap(std::string file_name) +{ + FILE *fp = fopen(file_name.c_str(), "w"); + + if (!fp) + { + return false; + } + + fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff); + for (unsigned int iy = 0; iy < size_y_; iy++) + { + for (unsigned int ix = 0; ix < size_x_; ix++) + { + unsigned char cost = getCost(ix, iy); + fprintf(fp, "%d ", cost); + } + fprintf(fp, "\n"); + } + fclose(fp); + return true; +} + +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_cloud.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_cloud.cpp new file mode 100644 index 0000000000..67bd2e1610 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_cloud.cpp @@ -0,0 +1,205 @@ +/* + * Copyright (C) 2009, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +static inline void mapToWorld3D(const unsigned int mx, const unsigned int my, const unsigned int mz, + const double origin_x, const double origin_y, const double origin_z, + const double x_resolution, const double y_resolution, const double z_resolution, + double& wx, double& wy, double& wz) +{ + // returns the center point of the cell + wx = origin_x + (mx + 0.5) * x_resolution; + wy = origin_y + (my + 0.5) * y_resolution; + wz = origin_z + (mz + 0.5) * z_resolution; +} + +struct Cell +{ + double x; + double y; + double z; + voxel_grid::VoxelStatus status; +}; +typedef std::vector V_Cell; + +float g_colors_r[] = {0.0f, 0.0f, 1.0f}; +float g_colors_g[] = {0.0f, 0.0f, 0.0f}; +float g_colors_b[] = {0.0f, 1.0f, 0.0f}; +float g_colors_a[] = {0.0f, 0.5f, 1.0f}; + +V_Cell g_marked; +V_Cell g_unknown; +void voxelCallback(const ros::Publisher& pub_marked, const ros::Publisher& pub_unknown, + const costmap_2d::VoxelGridConstPtr& grid) +{ + if (grid->data.empty()) + { + ROS_ERROR("Received empty voxel grid"); + return; + } + + ros::WallTime start = ros::WallTime::now(); + + ROS_DEBUG("Received voxel grid"); + const std::string frame_id = grid->header.frame_id; + const ros::Time stamp = grid->header.stamp; + const uint32_t* data = &grid->data.front(); + const double x_origin = grid->origin.x; + const double y_origin = grid->origin.y; + const double z_origin = grid->origin.z; + const double x_res = grid->resolutions.x; + const double y_res = grid->resolutions.y; + const double z_res = grid->resolutions.z; + const uint32_t x_size = grid->size_x; + const uint32_t y_size = grid->size_y; + const uint32_t z_size = grid->size_z; + + g_marked.clear(); + g_unknown.clear(); + uint32_t num_marked = 0; + uint32_t num_unknown = 0; + for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) + { + for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) + { + for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) + { + voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size, + data); + + if (status == voxel_grid::UNKNOWN) + { + Cell c; + c.status = status; + mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res, + z_res, c.x, c.y, c.z); + + g_unknown.push_back(c); + + ++num_unknown; + } + else if (status == voxel_grid::MARKED) + { + Cell c; + c.status = status; + mapToWorld3D(x_grid, y_grid, z_grid, x_origin, y_origin, z_origin, x_res, y_res, + z_res, c.x, c.y, c.z); + + g_marked.push_back(c); + + ++num_marked; + } + } + } + } + + { + sensor_msgs::PointCloud cloud; + cloud.points.resize(num_marked); + cloud.channels.resize(1); + cloud.channels[0].values.resize(num_marked); + cloud.channels[0].name = "rgb"; + cloud.header.frame_id = frame_id; + cloud.header.stamp = stamp; + + sensor_msgs::ChannelFloat32& chan = cloud.channels[0]; + for (uint32_t i = 0; i < num_marked; ++i) + { + geometry_msgs::Point32& p = cloud.points[i]; + float& cval = chan.values[i]; + Cell& c = g_marked[i]; + + p.x = c.x; + p.y = c.y; + p.z = c.z; + + uint32_t r = g_colors_r[c.status] * 255.0; + uint32_t g = g_colors_g[c.status] * 255.0; + uint32_t b = g_colors_b[c.status] * 255.0; + // uint32_t a = g_colors_a[c.status] * 255.0; + + uint32_t col = (r << 16) | (g << 8) | b; + cval = *reinterpret_cast(&col); + } + + pub_marked.publish(cloud); + } + + { + sensor_msgs::PointCloud cloud; + cloud.points.resize(num_unknown); + cloud.channels.resize(1); + cloud.channels[0].values.resize(num_unknown); + cloud.channels[0].name = "rgb"; + cloud.header.frame_id = frame_id; + cloud.header.stamp = stamp; + + sensor_msgs::ChannelFloat32& chan = cloud.channels[0]; + for (uint32_t i = 0; i < num_unknown; ++i) + { + geometry_msgs::Point32& p = cloud.points[i]; + float& cval = chan.values[i]; + Cell& c = g_unknown[i]; + + p.x = c.x; + p.y = c.y; + p.z = c.z; + + uint32_t r = g_colors_r[c.status] * 255.0; + uint32_t g = g_colors_g[c.status] * 255.0; + uint32_t b = g_colors_b[c.status] * 255.0; + // uint32_t a = g_colors_a[c.status] * 255.0; + + uint32_t col = (r << 16) | (g << 8) | b; + cval = *reinterpret_cast(&col); + } + + pub_unknown.publish(cloud); + } + + ros::WallTime end = ros::WallTime::now(); + ROS_DEBUG("Published %d points in %f seconds", num_marked + num_unknown, (end - start).toSec()); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "costmap_2d_cloud"); + ros::NodeHandle n; + + ROS_DEBUG("Startup"); + + ros::Publisher pub_marked = n.advertise < sensor_msgs::PointCloud > ("voxel_marked_cloud", 2); + ros::Publisher pub_unknown = n.advertise < sensor_msgs::PointCloud > ("voxel_unknown_cloud", 2); + ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid + > ("voxel_grid", 1, boost::bind(voxelCallback, pub_marked, pub_unknown, _1)); + + ros::spin(); +} diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_markers.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_markers.cpp new file mode 100644 index 0000000000..49d091c604 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_markers.cpp @@ -0,0 +1,153 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#include +#include +#include +#include + +struct Cell +{ + double x; + double y; + double z; + voxel_grid::VoxelStatus status; +}; +typedef std::vector V_Cell; + +float g_colors_r[] = {0.0f, 0.0f, 1.0f}; +float g_colors_g[] = {0.0f, 0.0f, 0.0f}; +float g_colors_b[] = {0.0f, 1.0f, 0.0f}; +float g_colors_a[] = {0.0f, 0.5f, 1.0f}; + +std::string g_marker_ns; +V_Cell g_cells; +void voxelCallback(const ros::Publisher& pub, const costmap_2d::VoxelGridConstPtr& grid) +{ + if (grid->data.empty()) + { + ROS_ERROR("Received empty voxel grid"); + return; + } + + ros::WallTime start = ros::WallTime::now(); + + ROS_DEBUG("Received voxel grid"); + const std::string frame_id = grid->header.frame_id; + const ros::Time stamp = grid->header.stamp; + const uint32_t* data = &grid->data.front(); + const double x_origin = grid->origin.x; + const double y_origin = grid->origin.y; + const double z_origin = grid->origin.z; + const double x_res = grid->resolutions.x; + const double y_res = grid->resolutions.y; + const double z_res = grid->resolutions.z; + const uint32_t x_size = grid->size_x; + const uint32_t y_size = grid->size_y; + const uint32_t z_size = grid->size_z; + + g_cells.clear(); + uint32_t num_markers = 0; + for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) + { + for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) + { + for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) + { + voxel_grid::VoxelStatus status = voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size, + data); + + if (status == voxel_grid::MARKED) + { + Cell c; + c.status = status; + c.x = x_origin + (x_grid + 0.5) * x_res; + c.y = y_origin + (y_grid + 0.5) * y_res; + c.z = z_origin + (z_grid + 0.5) * z_res; + g_cells.push_back(c); + + ++num_markers; + } + } + } + } + + visualization_msgs::Marker m; + m.header.frame_id = frame_id; + m.header.stamp = stamp; + m.ns = g_marker_ns; + m.id = 0; + m.type = visualization_msgs::Marker::CUBE_LIST; + m.action = visualization_msgs::Marker::ADD; + m.pose.orientation.w = 1.0; + m.scale.x = x_res; + m.scale.y = y_res; + m.scale.z = z_res; + m.color.r = g_colors_r[voxel_grid::MARKED]; + m.color.g = g_colors_g[voxel_grid::MARKED]; + m.color.b = g_colors_b[voxel_grid::MARKED]; + m.color.a = g_colors_a[voxel_grid::MARKED]; + m.points.resize(num_markers); + for (uint32_t i = 0; i < num_markers; ++i) + { + Cell& c = g_cells[i]; + geometry_msgs::Point& p = m.points[i]; + p.x = c.x; + p.y = c.y; + p.z = c.z; + } + + pub.publish(m); + + ros::WallTime end = ros::WallTime::now(); + ROS_DEBUG("Published %d markers in %f seconds", num_markers, (end - start).toSec()); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "costmap_2d_markers"); + ros::NodeHandle n; + + ROS_DEBUG("Startup"); + + ros::Publisher pub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1); + ros::Subscriber sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1)); + g_marker_ns = n.resolveName("voxel_grid"); + + ros::spin(); +} diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_node.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_node.cpp new file mode 100644 index 0000000000..ad8c3b0bc0 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_node.cpp @@ -0,0 +1,50 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "costmap_node"); + tf::TransformListener tf(ros::Duration(10)); + costmap_2d::Costmap2DROS lcr("costmap", tf); + + ros::spin(); + + return (0); +} diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_publisher.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_publisher.cpp new file mode 100644 index 0000000000..5a3ea0df5d --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_publisher.cpp @@ -0,0 +1,169 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include +#include + +namespace costmap_2d +{ + +char* Costmap2DPublisher::cost_translation_table_ = NULL; + +Costmap2DPublisher::Costmap2DPublisher(ros::NodeHandle * ros_node, Costmap2D* costmap, std::string global_frame, + std::string topic_name, bool always_send_full_costmap) : + node(ros_node), costmap_(costmap), global_frame_(global_frame), active_(false), + always_send_full_costmap_(always_send_full_costmap) +{ + costmap_pub_ = ros_node->advertise(topic_name, 1, + boost::bind(&Costmap2DPublisher::onNewSubscription, this, _1)); + costmap_update_pub_ = ros_node->advertise(topic_name + "_updates", 1); + + if (cost_translation_table_ == NULL) + { + cost_translation_table_ = new char[256]; + + // special values: + cost_translation_table_[0] = 0; // NO obstacle + cost_translation_table_[253] = 99; // INSCRIBED obstacle + cost_translation_table_[254] = 100; // LETHAL obstacle + cost_translation_table_[255] = -1; // UNKNOWN + + // regular cost values scale the range 1 to 252 (inclusive) to fit + // into 1 to 98 (inclusive). + for (int i = 1; i < 253; i++) + { + cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251); + } + } + + xn_ = yn_ = 0; + x0_ = costmap_->getSizeInCellsX(); + y0_ = costmap_->getSizeInCellsY(); +} + +Costmap2DPublisher::~Costmap2DPublisher() +{ +} + +void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub) +{ + prepareGrid(); + pub.publish(grid_); +} + +// prepare grid_ message for publication. +void Costmap2DPublisher::prepareGrid() +{ + boost::unique_lock lock(*(costmap_->getMutex())); + double resolution = costmap_->getResolution(); + + grid_.header.frame_id = global_frame_; + grid_.header.stamp = ros::Time::now(); + grid_.info.resolution = resolution; + + grid_.info.width = costmap_->getSizeInCellsX(); + grid_.info.height = costmap_->getSizeInCellsY(); + + double wx, wy; + costmap_->mapToWorld(0, 0, wx, wy); + grid_.info.origin.position.x = wx - resolution / 2; + grid_.info.origin.position.y = wy - resolution / 2; + grid_.info.origin.position.z = 0.0; + grid_.info.origin.orientation.w = 1.0; + saved_origin_x_ = costmap_->getOriginX(); + saved_origin_y_ = costmap_->getOriginY(); + + grid_.data.resize(grid_.info.width * grid_.info.height); + + unsigned char* data = costmap_->getCharMap(); + for (unsigned int i = 0; i < grid_.data.size(); i++) + { + grid_.data[i] = cost_translation_table_[ data[ i ]]; + } +} + +void Costmap2DPublisher::publishCostmap() +{ + if (costmap_pub_.getNumSubscribers() == 0) + { + // No subscribers, so why do any work? + return; + } + + float resolution = costmap_->getResolution(); + + if (always_send_full_costmap_ || grid_.info.resolution != resolution || + grid_.info.width != costmap_->getSizeInCellsX() || + grid_.info.height != costmap_->getSizeInCellsY() || + saved_origin_x_ != costmap_->getOriginX() || + saved_origin_y_ != costmap_->getOriginY()) + { + prepareGrid(); + costmap_pub_.publish(grid_); + } + else if (x0_ < xn_) + { + boost::unique_lock lock(*(costmap_->getMutex())); + // Publish Just an Update + map_msgs::OccupancyGridUpdate update; + update.header.stamp = ros::Time::now(); + update.header.frame_id = global_frame_; + update.x = x0_; + update.y = y0_; + update.width = xn_ - x0_; + update.height = yn_ - y0_; + update.data.resize(update.width * update.height); + + unsigned int i = 0; + for (unsigned int y = y0_; y < yn_; y++) + { + for (unsigned int x = x0_; x < xn_; x++) + { + unsigned char cost = costmap_->getCost(x, y); + update.data[i++] = cost_translation_table_[ cost ]; + } + } + costmap_update_pub_.publish(update); + } + + xn_ = yn_ = 0; + x0_ = costmap_->getSizeInCellsX(); + y0_ = costmap_->getSizeInCellsY(); +} + +} // end namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_ros.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_ros.cpp new file mode 100644 index 0000000000..88918ca6cf --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_2d_ros.cpp @@ -0,0 +1,572 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include +#include +#include +#include +#include + + +using namespace std; + +namespace costmap_2d +{ + +void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true) +{ + if (!old_h.hasParam(name)) + return; + + XmlRpc::XmlRpcValue value; + old_h.getParam(name, value); + new_h.setParam(name, value); + if (should_delete) old_h.deleteParam(name); +} + +Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) : + layered_costmap_(NULL), + name_(name), + tf_(tf), + transform_tolerance_(0.3), + map_update_thread_shutdown_(false), + stop_updates_(false), + initialized_(true), + stopped_(false), + robot_stopped_(false), + map_update_thread_(NULL), + last_publish_(0), + plugin_loader_("costmap_2d", "costmap_2d::Layer"), + publisher_(NULL), + dsrv_(NULL), + footprint_padding_(0.0) +{ + // Initialize old pose with something + old_pose_.setIdentity(); + old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30)); + + ros::NodeHandle private_nh("~/" + name); + ros::NodeHandle g_nh; + + // get our tf prefix + ros::NodeHandle prefix_nh; + std::string tf_prefix = tf::getPrefixParam(prefix_nh); + + // get two frames + private_nh.param("global_frame", global_frame_, std::string("/map")); + private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link")); + + // make sure that we set the frames appropriately based on the tf_prefix + global_frame_ = tf::resolve(tf_prefix, global_frame_); + robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_); + + ros::Time last_error = ros::Time::now(); + std::string tf_error; + // we need to make sure that the transform between the robot base frame and the global frame is available + while (ros::ok() + && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), ros::Duration(0.01), + &tf_error)) + { + ros::spinOnce(); + if (last_error + ros::Duration(5.0) < ros::Time::now()) + { + ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s", + robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); + last_error = ros::Time::now(); + } + // The error string will accumulate and errors will typically be the same, so the last + // will do for the warning above. Reset the string here to avoid accumulation. + tf_error.clear(); + } + + // check if we want a rolling window version of the costmap + bool rolling_window, track_unknown_space, always_send_full_costmap; + private_nh.param("rolling_window", rolling_window, false); + private_nh.param("track_unknown_space", track_unknown_space, false); + private_nh.param("always_send_full_costmap", always_send_full_costmap, false); + + layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space); + + if (!private_nh.hasParam("plugins")) + { + resetOldParameters(private_nh); + } + + if (private_nh.hasParam("plugins")) + { + XmlRpc::XmlRpcValue my_list; + private_nh.getParam("plugins", my_list); + for (int32_t i = 0; i < my_list.size(); ++i) + { + std::string pname = static_cast(my_list[i]["name"]); + std::string type = static_cast(my_list[i]["type"]); + ROS_INFO("Using plugin \"%s\"", pname.c_str()); + + boost::shared_ptr plugin = plugin_loader_.createInstance(type); + layered_costmap_->addPlugin(plugin); + plugin->initialize(layered_costmap_, name + "/" + pname, &tf_); + } + } + + // subscribe to the footprint topic + std::string topic_param, topic; + if (!private_nh.searchParam("footprint_topic", topic_param)) + { + topic_param = "footprint_topic"; + } + + private_nh.param(topic_param, topic, std::string("footprint")); + footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this); + + if (!private_nh.searchParam("published_footprint_topic", topic_param)) + { + topic_param = "published_footprint"; + } + + private_nh.param(topic_param, topic, std::string("oriented_footprint")); + footprint_pub_ = private_nh.advertise("footprint", 1); + + setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh)); + + publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap", + always_send_full_costmap); + + // create a thread to handle updating the map + stop_updates_ = false; + initialized_ = true; + stopped_ = false; + + // Create a time r to check if the robot is moving + robot_stopped_ = false; + timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this); + + dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name)); + dynamic_reconfigure::Server::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, + _2); + dsrv_->setCallback(cb); +} + +void Costmap2DROS::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint) +{ + setUnpaddedRobotFootprint(toPointVector(footprint)); +} + +Costmap2DROS::~Costmap2DROS() +{ + map_update_thread_shutdown_ = true; + if (map_update_thread_ != NULL) + { + map_update_thread_->join(); + delete map_update_thread_; + } + if (publisher_ != NULL) + delete publisher_; + + delete layered_costmap_; + delete dsrv_; +} + +void Costmap2DROS::resetOldParameters(ros::NodeHandle& nh) +{ + ROS_INFO("Loading from pre-hydro parameter style"); + bool flag; + std::string s; + std::vector < XmlRpc::XmlRpcValue > plugins; + + XmlRpc::XmlRpcValue::ValueStruct map; + SuperValue super_map; + SuperValue super_array; + + if (nh.getParam("static_map", flag) && flag) + { + map["name"] = XmlRpc::XmlRpcValue("static_layer"); + map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer"); + super_map.setStruct(&map); + plugins.push_back(super_map); + + ros::NodeHandle map_layer(nh, "static_layer"); + move_parameter(nh, map_layer, "map_topic"); + move_parameter(nh, map_layer, "unknown_cost_value"); + move_parameter(nh, map_layer, "lethal_cost_threshold"); + move_parameter(nh, map_layer, "track_unknown_space", false); + } + + ros::NodeHandle obstacles(nh, "obstacle_layer"); + if (nh.getParam("map_type", s) && s == "voxel") + { + map["name"] = XmlRpc::XmlRpcValue("obstacle_layer"); + map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer"); + super_map.setStruct(&map); + plugins.push_back(super_map); + + move_parameter(nh, obstacles, "origin_z"); + move_parameter(nh, obstacles, "z_resolution"); + move_parameter(nh, obstacles, "z_voxels"); + move_parameter(nh, obstacles, "mark_threshold"); + move_parameter(nh, obstacles, "unknown_threshold"); + move_parameter(nh, obstacles, "publish_voxel_map"); + } + else + { + map["name"] = XmlRpc::XmlRpcValue("obstacle_layer"); + map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer"); + super_map.setStruct(&map); + plugins.push_back(super_map); + } + + move_parameter(nh, obstacles, "max_obstacle_height"); + move_parameter(nh, obstacles, "raytrace_range"); + move_parameter(nh, obstacles, "obstacle_range"); + move_parameter(nh, obstacles, "track_unknown_space", true); + nh.param("observation_sources", s, std::string("")); + std::stringstream ss(s); + std::string source; + while (ss >> source) + { + move_parameter(nh, obstacles, source); + } + move_parameter(nh, obstacles, "observation_sources"); + + ros::NodeHandle inflation(nh, "inflation_layer"); + move_parameter(nh, inflation, "cost_scaling_factor"); + move_parameter(nh, inflation, "inflation_radius"); + map["name"] = XmlRpc::XmlRpcValue("inflation_layer"); + map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer"); + super_map.setStruct(&map); + plugins.push_back(super_map); + + super_array.setArray(&plugins); + nh.setParam("plugins", super_array); +} + +void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level) +{ + transform_tolerance_ = config.transform_tolerance; + if (map_update_thread_ != NULL) + { + map_update_thread_shutdown_ = true; + map_update_thread_->join(); + delete map_update_thread_; + } + map_update_thread_shutdown_ = false; + double map_update_frequency = config.update_frequency; + + double map_publish_frequency = config.publish_frequency; + if (map_publish_frequency > 0) + publish_cycle = ros::Duration(1 / map_publish_frequency); + else + publish_cycle = ros::Duration(-1); + + // find size parameters + double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x = + config.origin_x, + origin_y = config.origin_y; + + if (!layered_costmap_->isSizeLocked()) + { + layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution), + (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y); + } + + // If the padding has changed, call setUnpaddedRobotFootprint() to + // re-apply the padding. + if (footprint_padding_ != config.footprint_padding) + { + footprint_padding_ = config.footprint_padding; + setUnpaddedRobotFootprint(unpadded_footprint_); + } + + readFootprintFromConfig(config, old_config_); + + old_config_ = config; + + map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency)); +} + +void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, + const costmap_2d::Costmap2DConfig &old_config) +{ + // Only change the footprint if footprint or robot_radius has + // changed. Otherwise we might overwrite a footprint sent on a + // topic by a dynamic_reconfigure call which was setting some other + // variable. + if (new_config.footprint == old_config.footprint && + new_config.robot_radius == old_config.robot_radius) + { + return; + } + + if (new_config.footprint != "" && new_config.footprint != "[]") + { + std::vector new_footprint; + if (makeFootprintFromString(new_config.footprint, new_footprint)) + { + setUnpaddedRobotFootprint(new_footprint); + } + else + { + ROS_ERROR("Invalid footprint string from dynamic reconfigure"); + } + } + else + { + // robot_radius may be 0, but that must be intended at this point. + setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius)); + } +} + +void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector& points) +{ + unpadded_footprint_ = points; + padded_footprint_ = points; + padFootprint(padded_footprint_, footprint_padding_); + + layered_costmap_->setFootprint(padded_footprint_); +} + +void Costmap2DROS::movementCB(const ros::TimerEvent &event) +{ + // don't allow configuration to happen while this check occurs + // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_); + + tf::Stamped < tf::Pose > new_pose; + + if (!getRobotPose(new_pose)) + { + ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration"); + robot_stopped_ = false; + } + // make sure that the robot is not moving + else if (fabs((old_pose_.getOrigin() - new_pose.getOrigin()).length()) < 1e-3 + && fabs(old_pose_.getRotation().angle(new_pose.getRotation())) < 1e-3) + { + old_pose_ = new_pose; + robot_stopped_ = true; + } + else + { + old_pose_ = new_pose; + robot_stopped_ = false; + } +} + +void Costmap2DROS::mapUpdateLoop(double frequency) +{ + // the user might not want to run the loop every cycle + if (frequency == 0.0) + return; + + ros::NodeHandle nh; + ros::Rate r(frequency); + while (nh.ok() && !map_update_thread_shutdown_) + { + struct timeval start, end; + double start_t, end_t, t_diff; + gettimeofday(&start, NULL); + + updateMap(); + + gettimeofday(&end, NULL); + start_t = start.tv_sec + double(start.tv_usec) / 1e6; + end_t = end.tv_sec + double(end.tv_usec) / 1e6; + t_diff = end_t - start_t; + ROS_DEBUG("Map update time: %.9f", t_diff); + if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized()) + { + unsigned int x0, y0, xn, yn; + layered_costmap_->getBounds(&x0, &xn, &y0, &yn); + publisher_->updateBounds(x0, xn, y0, yn); + + ros::Time now = ros::Time::now(); + if (last_publish_ + publish_cycle < now) + { + publisher_->publishCostmap(); + last_publish_ = now; + } + } + r.sleep(); + // make sure to sleep for the remainder of our cycle time + if (r.cycleTime() > ros::Duration(1 / frequency)) + ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency, + r.cycleTime().toSec()); + } +} + +void Costmap2DROS::updateMap() +{ + if (!stop_updates_) + { + // get global pose + tf::Stamped < tf::Pose > pose; + if (getRobotPose (pose)) + { + double x = pose.getOrigin().x(), + y = pose.getOrigin().y(), + yaw = tf::getYaw(pose.getRotation()); + + layered_costmap_->updateMap(x, y, yaw); + + geometry_msgs::PolygonStamped footprint; + footprint.header.frame_id = global_frame_; + footprint.header.stamp = ros::Time::now(); + transformFootprint(x, y, yaw, padded_footprint_, footprint); + footprint_pub_.publish(footprint); + + initialized_ = true; + } + } +} + +void Costmap2DROS::start() +{ + std::vector < boost::shared_ptr > *plugins = layered_costmap_->getPlugins(); + // check if we're stopped or just paused + if (stopped_) + { + // if we're stopped we need to re-subscribe to topics + for (vector >::iterator plugin = plugins->begin(); plugin != plugins->end(); + ++plugin) + { + (*plugin)->activate(); + } + stopped_ = false; + } + stop_updates_ = false; + + // block until the costmap is re-initialized.. meaning one update cycle has run + ros::Rate r(100.0); + while (ros::ok() && !initialized_) + r.sleep(); +} + +void Costmap2DROS::stop() +{ + stop_updates_ = true; + std::vector < boost::shared_ptr > *plugins = layered_costmap_->getPlugins(); + // unsubscribe from topics + for (vector >::iterator plugin = plugins->begin(); plugin != plugins->end(); + ++plugin) + { + (*plugin)->deactivate(); + } + initialized_ = false; + stopped_ = true; +} + +void Costmap2DROS::pause() +{ + stop_updates_ = true; + initialized_ = false; +} + +void Costmap2DROS::resume() +{ + stop_updates_ = false; + + // block until the costmap is re-initialized.. meaning one update cycle has run + ros::Rate r(100.0); + while (!initialized_) + r.sleep(); +} + + +void Costmap2DROS::resetLayers() +{ + Costmap2D* top = layered_costmap_->getCostmap(); + top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY()); + std::vector < boost::shared_ptr > *plugins = layered_costmap_->getPlugins(); + for (vector >::iterator plugin = plugins->begin(); plugin != plugins->end(); + ++plugin) + { + (*plugin)->reset(); + } +} + +bool Costmap2DROS::getRobotPose(tf::Stamped& global_pose) const +{ + global_pose.setIdentity(); + tf::Stamped < tf::Pose > robot_pose; + robot_pose.setIdentity(); + robot_pose.frame_id_ = robot_base_frame_; + robot_pose.stamp_ = ros::Time(); + ros::Time current_time = ros::Time::now(); // save time for checking tf delay later + + // get the global pose of the robot + try + { + tf_.transformPose(global_frame_, robot_pose, global_pose); + } + catch (tf::LookupException& ex) + { + ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); + return false; + } + catch (tf::ConnectivityException& ex) + { + ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); + return false; + } + catch (tf::ExtrapolationException& ex) + { + ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); + return false; + } + // check global_pose timeout + if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) + { + ROS_WARN_THROTTLE(1.0, + "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", + current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_); + return false; + } + + return true; +} + +void Costmap2DROS::getOrientedFootprint(std::vector& oriented_footprint) const +{ + tf::Stamped global_pose; + if (!getRobotPose(global_pose)) + return; + + double yaw = tf::getYaw(global_pose.getRotation()); + transformFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw, + padded_footprint_, oriented_footprint); +} + +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_layer.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_layer.cpp new file mode 100644 index 0000000000..14f04c4649 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_layer.cpp @@ -0,0 +1,142 @@ +#include + +namespace costmap_2d +{ + +void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y) +{ + *min_x = std::min(x, *min_x); + *min_y = std::min(y, *min_y); + *max_x = std::max(x, *max_x); + *max_y = std::max(y, *max_y); +} + +void CostmapLayer::matchSize() +{ + Costmap2D* master = layered_costmap_->getCostmap(); + resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), + master->getOriginX(), master->getOriginY()); +} + +void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1) +{ + extra_min_x_ = std::min(mx0, extra_min_x_); + extra_max_x_ = std::max(mx1, extra_max_x_); + extra_min_y_ = std::min(my0, extra_min_y_); + extra_max_y_ = std::max(my1, extra_max_y_); + has_extra_bounds_ = true; +} + +void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y) +{ + if (!has_extra_bounds_) + return; + + *min_x = std::min(extra_min_x_, *min_x); + *min_y = std::min(extra_min_y_, *min_y); + *max_x = std::max(extra_max_x_, *max_x); + *max_y = std::max(extra_max_y_, *max_y); + extra_min_x_ = 1e6; + extra_min_y_ = 1e6; + extra_max_x_ = -1e6; + extra_max_y_ = -1e6; + has_extra_bounds_ = false; +} + +void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) + return; + + unsigned char* master_array = master_grid.getCharMap(); + unsigned int span = master_grid.getSizeInCellsX(); + + for (int j = min_j; j < max_j; j++) + { + unsigned int it = j * span + min_i; + for (int i = min_i; i < max_i; i++) + { + if (costmap_[it] == NO_INFORMATION){ + it++; + continue; + } + + unsigned char old_cost = master_array[it]; + if (old_cost == NO_INFORMATION || old_cost < costmap_[it]) + master_array[it] = costmap_[it]; + it++; + } + } +} + +void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, + int max_i, int max_j) +{ + if (!enabled_) + return; + unsigned char* master = master_grid.getCharMap(); + unsigned int span = master_grid.getSizeInCellsX(); + + for (int j = min_j; j < max_j; j++) + { + unsigned int it = span*j+min_i; + for (int i = min_i; i < max_i; i++) + { + master[it] = costmap_[it]; + it++; + } + } +} + +void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) + return; + unsigned char* master = master_grid.getCharMap(); + unsigned int span = master_grid.getSizeInCellsX(); + + for (int j = min_j; j < max_j; j++) + { + unsigned int it = span*j+min_i; + for (int i = min_i; i < max_i; i++) + { + if (costmap_[it] != NO_INFORMATION) + master[it] = costmap_[it]; + it++; + } + } +} + +void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) + return; + unsigned char* master_array = master_grid.getCharMap(); + unsigned int span = master_grid.getSizeInCellsX(); + + for (int j = min_j; j < max_j; j++) + { + unsigned int it = j * span + min_i; + for (int i = min_i; i < max_i; i++) + { + if (costmap_[it] == NO_INFORMATION){ + it++; + continue; + } + + unsigned char old_cost = master_array[it]; + if (old_cost == NO_INFORMATION) + master_array[it] = costmap_[it]; + else + { + int sum = old_cost + costmap_[it]; + if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; + else + master_array[it] = sum; + } + it++; + } + } +} +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/costmap_math.cpp b/src/libs/nav2_util/src/costmap_2d/src/costmap_math.cpp new file mode 100644 index 0000000000..97f7f50699 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/costmap_math.cpp @@ -0,0 +1,89 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1) +{ + double A = pX - x0; + double B = pY - y0; + double C = x1 - x0; + double D = y1 - y0; + + double dot = A * C + B * D; + double len_sq = C * C + D * D; + double param = dot / len_sq; + + double xx, yy; + + if (param < 0) + { + xx = x0; + yy = y0; + } + else if (param > 1) + { + xx = x1; + yy = y1; + } + else + { + xx = x0 + param * C; + yy = y0 + param * D; + } + + return distance(pX, pY, xx, yy); +} + +bool intersects(std::vector& polygon, float testx, float testy) +{ + bool c = false; + int i, j, nvert = polygon.size(); + for (i = 0, j = nvert - 1; i < nvert; j = i++) + { + float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x; + + if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi)) + c = !c; + } + return c; +} + +bool intersects_helper(std::vector& polygon1, std::vector& polygon2) +{ + for (unsigned int i = 0; i < polygon1.size(); i++) + if (intersects(polygon2, polygon1[i].x, polygon1[i].y)) + return true; + return false; +} + +bool intersects(std::vector& polygon1, std::vector& polygon2) +{ + return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1); +} diff --git a/src/libs/nav2_util/src/costmap_2d/src/footprint.cpp b/src/libs/nav2_util/src/costmap_2d/src/footprint.cpp new file mode 100644 index 0000000000..dc51700a2f --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/footprint.cpp @@ -0,0 +1,325 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace costmap_2d +{ + +void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist) +{ + min_dist = std::numeric_limits::max(); + max_dist = 0.0; + + if (footprint.size() <= 2) + { + return; + } + + for (unsigned int i = 0; i < footprint.size() - 1; ++i) + { + // check the distance from the robot center point to the first vertex + double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y); + double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y, + footprint[i + 1].x, footprint[i + 1].y); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); + } + + // we also need to do the last vertex and the first vertex + double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y); + double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y, + footprint.front().x, footprint.front().y); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); +} + +geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt) +{ + geometry_msgs::Point32 point32; + point32.x = pt.x; + point32.y = pt.y; + point32.z = pt.z; + return point32; +} + +geometry_msgs::Point toPoint(geometry_msgs::Point32 pt) +{ + geometry_msgs::Point point; + point.x = pt.x; + point.y = pt.y; + point.z = pt.z; + return point; +} + +geometry_msgs::Polygon toPolygon(std::vector pts) +{ + geometry_msgs::Polygon polygon; + for (int i = 0; i < pts.size(); i++){ + polygon.points.push_back(toPoint32(pts[i])); + } + return polygon; +} + +std::vector toPointVector(geometry_msgs::Polygon polygon) +{ + std::vector pts; + for (int i = 0; i < polygon.points.size(); i++) + { + pts.push_back(toPoint(polygon.points[i])); + } + return pts; +} + +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + std::vector& oriented_footprint) +{ + // build the oriented footprint at a given location + oriented_footprint.clear(); + double cos_th = cos(theta); + double sin_th = sin(theta); + for (unsigned int i = 0; i < footprint_spec.size(); ++i) + { + geometry_msgs::Point new_pt; + new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); + new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); + oriented_footprint.push_back(new_pt); + } +} + +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + geometry_msgs::PolygonStamped& oriented_footprint) +{ + // build the oriented footprint at a given location + oriented_footprint.polygon.points.clear(); + double cos_th = cos(theta); + double sin_th = sin(theta); + for (unsigned int i = 0; i < footprint_spec.size(); ++i) + { + geometry_msgs::Point32 new_pt; + new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); + new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); + oriented_footprint.polygon.points.push_back(new_pt); + } +} + +void padFootprint(std::vector& footprint, double padding) +{ + // pad footprint in place + for (unsigned int i = 0; i < footprint.size(); i++) + { + geometry_msgs::Point& pt = footprint[ i ]; + pt.x += sign0(pt.x) * padding; + pt.y += sign0(pt.y) * padding; + } +} + + +std::vector makeFootprintFromRadius(double radius) +{ + std::vector points; + + // Loop over 16 angles around a circle making a point each time + int N = 16; + geometry_msgs::Point pt; + for (int i = 0; i < N; ++i) + { + double angle = i * 2 * M_PI / N; + pt.x = cos(angle) * radius; + pt.y = sin(angle) * radius; + + points.push_back(pt); + } + + return points; +} + + +bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint) +{ + std::string error; + std::vector > vvf = parseVVF(footprint_string, error); + + if (error != "") + { + ROS_ERROR("Error parsing footprint parameter: '%s'", error.c_str()); + ROS_ERROR(" Footprint string was '%s'.", footprint_string.c_str()); + return false; + } + + // convert vvf into points. + if (vvf.size() < 3) + { + ROS_ERROR("You must specify at least three points for the robot footprint, reverting to previous footprint."); + return false; + } + footprint.reserve(vvf.size()); + for (unsigned int i = 0; i < vvf.size(); i++) + { + if (vvf[ i ].size() == 2) + { + geometry_msgs::Point point; + point.x = vvf[ i ][ 0 ]; + point.y = vvf[ i ][ 1 ]; + point.z = 0; + footprint.push_back(point); + } + else + { + ROS_ERROR("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", + int(vvf[ i ].size())); + return false; + } + } + + return true; +} + + + +std::vector makeFootprintFromParams(ros::NodeHandle& nh) +{ + std::string full_param_name; + std::string full_radius_param_name; + std::vector points; + + if (nh.searchParam("footprint", full_param_name)) + { + XmlRpc::XmlRpcValue footprint_xmlrpc; + nh.getParam(full_param_name, footprint_xmlrpc); + if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString && + footprint_xmlrpc != "" && footprint_xmlrpc != "[]") + { + if (makeFootprintFromString(std::string(footprint_xmlrpc), points)) + { + writeFootprintToParam(nh, points); + return points; + } + } + else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name); + writeFootprintToParam(nh, points); + return points; + } + } + + if (nh.searchParam("robot_radius", full_radius_param_name)) + { + double robot_radius; + nh.param(full_radius_param_name, robot_radius, 1.234); + points = makeFootprintFromRadius(robot_radius); + nh.setParam("robot_radius", robot_radius); + } + // Else neither param was found anywhere this knows about, so + // defaults will come from dynamic_reconfigure stuff, set in + // cfg/Costmap2D.cfg and read in this file in reconfigureCB(). + return points; +} + +void writeFootprintToParam(ros::NodeHandle& nh, const std::vector& footprint) +{ + std::ostringstream oss; + bool first = true; + for (unsigned int i = 0; i < footprint.size(); i++) + { + geometry_msgs::Point p = footprint[ i ]; + if (first) + { + oss << "[[" << p.x << "," << p.y << "]"; + first = false; + } + else + { + oss << ",[" << p.x << "," << p.y << "]"; + } + } + oss << "]"; + nh.setParam("footprint", oss.str().c_str()); +} + +double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) +{ + // Make sure that the value we're looking at is either a double or an int. + if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && + value.getType() != XmlRpc::XmlRpcValue::TypeDouble) + { + std::string& value_string = value; + ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.", + full_param_name.c_str(), value_string.c_str()); + throw std::runtime_error("Values in the footprint specification must be numbers"); + } + return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); +} + +std::vector makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, + const std::string& full_param_name) +{ + // Make sure we have an array of at least 3 elements. + if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || + footprint_xmlrpc.size() < 3) + { + ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", + full_param_name.c_str(), std::string(footprint_xmlrpc).c_str()); + throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least " + "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); + } + + std::vector footprint; + geometry_msgs::Point pt; + + for (int i = 0; i < footprint_xmlrpc.size(); ++i) + { + // Make sure each element of the list is an array of size 2. (x and y coordinates) + XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ]; + if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || + point.size() != 2) + { + ROS_FATAL("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", + full_param_name.c_str()); + throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); + } + + pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name); + pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name); + + footprint.push_back(pt); + } + return footprint; +} + +} // end namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/layer.cpp b/src/libs/nav2_util/src/costmap_2d/src/layer.cpp new file mode 100644 index 0000000000..1b7d699b5f --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/layer.cpp @@ -0,0 +1,56 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "costmap_2d/layer.h" + +namespace costmap_2d +{ + +Layer::Layer() + : layered_costmap_(NULL) + , current_(false) + , enabled_(false) + , name_() + , tf_(NULL) +{} + +void Layer::initialize(LayeredCostmap* parent, std::string name, tf::TransformListener *tf) +{ + layered_costmap_ = parent; + name_ = name; + tf_ = tf; + onInitialize(); +} + +const std::vector& Layer::getFootprint() const +{ + return layered_costmap_->getFootprint(); +} + +} // end namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/layered_costmap.cpp b/src/libs/nav2_util/src/costmap_2d/src/layered_costmap.cpp new file mode 100644 index 0000000000..2ca96b8245 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/layered_costmap.cpp @@ -0,0 +1,185 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#include +#include +#include +#include +#include +#include + +using std::vector; + +namespace costmap_2d +{ + +LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) : + costmap_(), + global_frame_(global_frame), + rolling_window_(rolling_window), + current_(false), + minx_(0.0), + miny_(0.0), + maxx_(0.0), + maxy_(0.0), + bx0_(0), + bxn_(0), + by0_(0), + byn_(0), + initialized_(false), + size_locked_(false), + circumscribed_radius_(1.0), + inscribed_radius_(0.1) +{ + if (track_unknown) + costmap_.setDefaultValue(255); + else + costmap_.setDefaultValue(0); +} + +LayeredCostmap::~LayeredCostmap() +{ + while (plugins_.size() > 0) + { + plugins_.pop_back(); + } +} + +void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, + double origin_y, bool size_locked) +{ + boost::unique_lock lock(*(costmap_.getMutex())); + size_locked_ = size_locked; + costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->matchSize(); + } +} + +void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) +{ + // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) + // implement thread unsafe updateBounds() functions. + boost::unique_lock lock(*(costmap_.getMutex())); + + // if we're using a rolling buffer costmap... we need to update the origin using the robot's position + if (rolling_window_) + { + double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2; + double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2; + costmap_.updateOrigin(new_origin_x, new_origin_y); + } + + if (plugins_.size() == 0) + return; + + minx_ = miny_ = 1e30; + maxx_ = maxy_ = -1e30; + + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + double prev_minx = minx_; + double prev_miny = miny_; + double prev_maxx = maxx_; + double prev_maxy = maxy_; + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); + if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) + { + ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " + "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", + prev_minx, prev_miny, prev_maxx , prev_maxy, + minx_, miny_, maxx_ , maxy_, + (*plugin)->getName().c_str()); + } + } + + int x0, xn, y0, yn; + costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); + costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + + x0 = std::max(0, x0); + xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1); + y0 = std::max(0, y0); + yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1); + + ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); + + if (xn < x0 || yn < y0) + return; + + costmap_.resetMap(x0, y0, xn, yn); + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->updateCosts(costmap_, x0, y0, xn, yn); + } + + bx0_ = x0; + bxn_ = xn; + by0_ = y0; + byn_ = yn; + + initialized_ = true; +} + +bool LayeredCostmap::isCurrent() +{ + current_ = true; + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + current_ = current_ && (*plugin)->isCurrent(); + } + return current_; +} + +void LayeredCostmap::setFootprint(const std::vector& footprint_spec) +{ + footprint_ = footprint_spec; + costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); + + for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); + ++plugin) + { + (*plugin)->onFootprintChanged(); + } +} + +} // namespace costmap_2d diff --git a/src/libs/nav2_util/src/costmap_2d/src/observation_buffer.cpp b/src/libs/nav2_util/src/costmap_2d/src/observation_buffer.cpp new file mode 100644 index 0000000000..a8dbd94fe9 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/src/observation_buffer.cpp @@ -0,0 +1,258 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#include + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace tf; + +namespace costmap_2d +{ +ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate, + double min_obstacle_height, double max_obstacle_height, double obstacle_range, + double raytrace_range, TransformListener& tf, string global_frame, + string sensor_frame, double tf_tolerance) : + tf_(tf), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate), + last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name), + min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height), + obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance) +{ +} + +ObservationBuffer::~ObservationBuffer() +{ +} + +bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) +{ + ros::Time transform_time = ros::Time::now(); + std::string tf_error; + + if (!tf_.waitForTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), + ros::Duration(0.01), &tf_error)) + { + ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(), + global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); + return false; + } + + list::iterator obs_it; + for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) + { + try + { + Observation& obs = *obs_it; + + geometry_msgs::PointStamped origin; + origin.header.frame_id = global_frame_; + origin.header.stamp = transform_time; + origin.point = obs.origin_; + + // we need to transform the origin of the observation to the new global frame + tf_.transformPoint(new_global_frame, origin, origin); + obs.origin_ = origin.point; + + // we also need to transform the cloud of the observation to the new global frame + pcl_ros::transformPointCloud(new_global_frame, *obs.cloud_, *obs.cloud_, tf_); + } + catch (TransformException& ex) + { + ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(), + new_global_frame.c_str(), ex.what()); + return false; + } + } + + // now we need to update our global_frame member + global_frame_ = new_global_frame; + return true; +} + +void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) +{ + try + { + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::toPCL(cloud, pcl_pc2); + // Actually convert the PointCloud2 message into a type we can reason about + pcl::PointCloud < pcl::PointXYZ > pcl_cloud; + pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud); + bufferCloud(pcl_cloud); + } + catch (pcl::PCLException& ex) + { + ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what()); + return; + } +} + +void ObservationBuffer::bufferCloud(const pcl::PointCloud& cloud) +{ + Stamped < tf::Vector3 > global_origin; + + // create a new observation on the list to be populated + observation_list_.push_front(Observation()); + + // check whether the origin frame has been set explicitly or whether we should get it from the cloud + string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; + + try + { + // given these observations come from sensors... we'll need to store the origin pt of the sensor + Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0), + pcl_conversions::fromPCL(cloud.header).stamp, origin_frame); + tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5)); + tf_.transformPoint(global_frame_, local_origin, global_origin); + observation_list_.front().origin_.x = global_origin.getX(); + observation_list_.front().origin_.y = global_origin.getY(); + observation_list_.front().origin_.z = global_origin.getZ(); + + // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations + observation_list_.front().raytrace_range_ = raytrace_range_; + observation_list_.front().obstacle_range_ = obstacle_range_; + + pcl::PointCloud < pcl::PointXYZ > global_frame_cloud; + + // transform the point cloud + pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_); + global_frame_cloud.header.stamp = cloud.header.stamp; + + // now we need to remove observations from the cloud that are below or above our height thresholds + pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_); + unsigned int cloud_size = global_frame_cloud.points.size(); + observation_cloud.points.resize(cloud_size); + unsigned int point_count = 0; + + // copy over the points that are within our height bounds + for (unsigned int i = 0; i < cloud_size; ++i) + { + if (global_frame_cloud.points[i].z <= max_obstacle_height_ + && global_frame_cloud.points[i].z >= min_obstacle_height_) + { + observation_cloud.points[point_count++] = global_frame_cloud.points[i]; + } + } + + // resize the cloud for the number of legal points + observation_cloud.points.resize(point_count); + observation_cloud.header.stamp = cloud.header.stamp; + observation_cloud.header.frame_id = global_frame_cloud.header.frame_id; + } + catch (TransformException& ex) + { + // if an exception occurs, we need to remove the empty observation from the list + observation_list_.pop_front(); + ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), + cloud.header.frame_id.c_str(), ex.what()); + return; + } + + // if the update was successful, we want to update the last updated time + last_updated_ = ros::Time::now(); + + // we'll also remove any stale observations from the list + purgeStaleObservations(); +} + +// returns a copy of the observations +void ObservationBuffer::getObservations(vector& observations) +{ + // first... let's make sure that we don't have any stale observations + purgeStaleObservations(); + + // now we'll just copy the observations for the caller + list::iterator obs_it; + for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) + { + observations.push_back(*obs_it); + } +} + +void ObservationBuffer::purgeStaleObservations() +{ + if (!observation_list_.empty()) + { + list::iterator obs_it = observation_list_.begin(); + // if we're keeping observations for no time... then we'll only keep one observation + if (observation_keep_time_ == ros::Duration(0.0)) + { + observation_list_.erase(++obs_it, observation_list_.end()); + return; + } + + // otherwise... we'll have to loop through the observations to see which ones are stale + for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) + { + Observation& obs = *obs_it; + // check if the observation is out of date... and if it is, remove it and those that follow from the list + ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp; + if ((last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp) > observation_keep_time_) + { + observation_list_.erase(obs_it, observation_list_.end()); + return; + } + } + } +} + +bool ObservationBuffer::isCurrent() const +{ + if (expected_update_rate_ == ros::Duration(0.0)) + return true; + + bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec(); + if (!current) + { + ROS_WARN( + "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", + topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec()); + } + return current; +} + +void ObservationBuffer::resetLastUpdated() +{ + last_updated_ = ros::Time::now(); +} +} // namespace costmap_2d + diff --git a/src/libs/nav2_util/src/costmap_2d/test/TenByTen.pgm b/src/libs/nav2_util/src/costmap_2d/test/TenByTen.pgm new file mode 100644 index 0000000000000000000000000000000000000000..558e3e850d51c970535c18a940d16164249710f0 GIT binary patch literal 152 zcmWGA9-2um$0&dkqKFw`^TGBi*yG~hBa hHRbw`8%Rk>fh3Lp|3_hM-i%cXR1H*)fq? + +#include "costmap_2d/array_parser.h" + +using namespace costmap_2d; + +TEST(array_parser, basic_operation) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[[1, 2.2], [.3, -4e4]]", error ); + EXPECT_EQ( 2, vvf.size() ); + EXPECT_EQ( 2, vvf[0].size() ); + EXPECT_EQ( 2, vvf[1].size() ); + EXPECT_EQ( 1.0f, vvf[0][0] ); + EXPECT_EQ( 2.2f, vvf[0][1] ); + EXPECT_EQ( 0.3f, vvf[1][0] ); + EXPECT_EQ( -40000.0f, vvf[1][1] ); + EXPECT_EQ( "", error ); +} + +TEST(array_parser, missing_open) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[1, 2.2], [.3, -4e4]]", error ); + EXPECT_FALSE( error == "" ); +} + +TEST(array_parser, missing_close) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[[1, 2.2], [.3, -4e4]", error ); + EXPECT_FALSE( error == "" ); +} + +TEST(array_parser, wrong_depth) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[1, 2.2], [.3, -4e4]", error ); + EXPECT_FALSE( error == "" ); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest( &argc, argv ); + return RUN_ALL_TESTS(); +} + diff --git a/src/libs/nav2_util/src/costmap_2d/test/costmap_params.yaml b/src/libs/nav2_util/src/costmap_2d/test/costmap_params.yaml new file mode 100644 index 0000000000..25705e4602 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/costmap_params.yaml @@ -0,0 +1,29 @@ +global_frame: /map +robot_base_frame: base_link +update_frequency: 5.0 +publish_frequency: 0.0 +static_map: true +rolling_window: false + +#START VOXEL STUFF +map_type: voxel +origin_z: 0.0 +z_resolution: 0.2 +z_voxels: 10 +unknown_threshold: 10 +mark_threshold: 0 +#END VOXEL STUFF + +transform_tolerance: 0.3 +obstacle_range: 2.5 +max_obstacle_height: 2.0 +raytrace_range: 3.0 +footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] +#robot_radius: 0.46 +footprint_padding: 0.01 +inflation_radius: 0.55 +cost_scaling_factor: 10.0 +lethal_cost_threshold: 100 +observation_sources: base_scan +base_scan: {data_type: LaserScan, expected_update_rate: 0.4, + observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} diff --git a/src/libs/nav2_util/src/costmap_2d/test/costmap_tester.cpp b/src/libs/nav2_util/src/costmap_2d/test/costmap_tester.cpp new file mode 100644 index 0000000000..ff7aaabef8 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/costmap_tester.cpp @@ -0,0 +1,145 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ +#include +#include +#include +#include +#include + +namespace costmap_2d { + +class CostmapTester : public testing::Test { + public: + CostmapTester(tf::TransformListener& tf); + void checkConsistentCosts(); + void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y); + void compareCells(costmap_2d::Costmap2D& costmap, + unsigned int x, unsigned int y, unsigned int nx, unsigned int ny); + virtual void TestBody(){} + + private: + costmap_2d::Costmap2DROS costmap_ros_; +}; + +CostmapTester::CostmapTester(tf::TransformListener& tf): costmap_ros_("test_costmap", tf){} + +void CostmapTester::checkConsistentCosts(){ + costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap(); + + //get a copy of the costmap contained by our ros wrapper + costmap->saveMap("costmap_test.pgm"); + + //loop through the costmap and check for any unexpected drop-offs in costs + for(unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i){ + for(unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j){ + compareCellToNeighbors(*costmap, i, j); + } + } +} + +void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){ + //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable + for(int offset_x = -1; offset_x <= 1; ++offset_x){ + for(int offset_y = -1; offset_y <= 1; ++offset_y){ + int nx = x + offset_x; + int ny = y + offset_y; + + //check to make sure that the neighbor cell is a legal one + if(nx >= 0 && nx < (int)costmap.getSizeInCellsX() && ny >=0 && ny < (int)costmap.getSizeInCellsY()){ + compareCells(costmap, x, y, nx, ny); + } + } + } +} + +//for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect +void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){ + double cell_distance = hypot(static_cast(x-nx), static_cast(y-ny)); + + unsigned char cell_cost = costmap.getCost(x, y); + unsigned char neighbor_cost = costmap.getCost(nx, ny); + + if(cell_cost == costmap_2d::LETHAL_OBSTACLE){ + //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost + unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance); + EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE)); + } + else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away + double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1; + unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance); + if(neighbor_cost < expected_lowest_cost){ + ROS_ERROR("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f", + x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance); + ROS_ERROR("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny); + costmap.saveMap("failing_costmap.pgm"); + } + EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE)); + } +} +}; + +costmap_2d::CostmapTester* map_tester = NULL; + +TEST(CostmapTester, checkConsistentCosts){ + map_tester->checkConsistentCosts(); +} + +void testCallback(const ros::TimerEvent& e){ + int test_result = RUN_ALL_TESTS(); + ROS_INFO("gtest return value: %d", test_result); + ros::shutdown(); +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "costmap_tester_node"); + testing::InitGoogleTest(&argc, argv); + + ros::NodeHandle n; + ros::NodeHandle private_nh("~"); + + tf::TransformListener tf(ros::Duration(10)); + map_tester = new costmap_2d::CostmapTester(tf); + + double wait_time; + private_nh.param("wait_time", wait_time, 30.0); + ros::Timer timer = n.createTimer(ros::Duration(wait_time), testCallback); + + ros::spin(); + + return(0); +} diff --git a/src/libs/nav2_util/src/costmap_2d/test/footprint_tests.cpp b/src/libs/nav2_util/src/costmap_2d/test/footprint_tests.cpp new file mode 100644 index 0000000000..b7a70a579c --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/footprint_tests.cpp @@ -0,0 +1,178 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Dave Hershberger +*********************************************************************/ +#include +#include +#include + +using namespace costmap_2d; + +tf::TransformListener* tf_; + +TEST( Costmap2DROS, unpadded_footprint_from_string_param ) +{ + Costmap2DROS cm( "unpadded/string", *tf_ ); + std::vector footprint = cm.getRobotFootprint(); + EXPECT_EQ( 3, footprint.size() ); + + EXPECT_EQ( 1.0f, footprint[ 0 ].x ); + EXPECT_EQ( 1.0f, footprint[ 0 ].y ); + EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + + EXPECT_EQ( -1.0f, footprint[ 1 ].x ); + EXPECT_EQ( 1.0f, footprint[ 1 ].y ); + EXPECT_EQ( 0.0f, footprint[ 1 ].z ); + + EXPECT_EQ( -1.0f, footprint[ 2 ].x ); + EXPECT_EQ( -1.0f, footprint[ 2 ].y ); + EXPECT_EQ( 0.0f, footprint[ 2 ].z ); +} + +TEST( Costmap2DROS, padded_footprint_from_string_param ) +{ + Costmap2DROS cm( "padded/string", *tf_ ); + std::vector footprint = cm.getRobotFootprint(); + EXPECT_EQ( 3, footprint.size() ); + + EXPECT_EQ( 1.5f, footprint[ 0 ].x ); + EXPECT_EQ( 1.5f, footprint[ 0 ].y ); + EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + + EXPECT_EQ( -1.5f, footprint[ 1 ].x ); + EXPECT_EQ( 1.5f, footprint[ 1 ].y ); + EXPECT_EQ( 0.0f, footprint[ 1 ].z ); + + EXPECT_EQ( -1.5f, footprint[ 2 ].x ); + EXPECT_EQ( -1.5f, footprint[ 2 ].y ); + EXPECT_EQ( 0.0f, footprint[ 2 ].z ); +} + +TEST( Costmap2DROS, radius_param ) +{ + Costmap2DROS cm( "radius/sub", *tf_ ); + std::vector footprint = cm.getRobotFootprint(); + // Circular robot has 16-point footprint auto-generated. + EXPECT_EQ( 16, footprint.size() ); + + // Check the first point + EXPECT_EQ( 10.0f, footprint[ 0 ].x ); + EXPECT_EQ( 0.0f, footprint[ 0 ].y ); + EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + + // Check the 4th point, which should be 90 degrees around the circle from the first. + EXPECT_NEAR( 0.0f, footprint[ 4 ].x, 0.0001 ); + EXPECT_NEAR( 10.0f, footprint[ 4 ].y, 0.0001 ); + EXPECT_EQ( 0.0f, footprint[ 4 ].z ); +} + +TEST( Costmap2DROS, footprint_from_xmlrpc_param ) +{ + Costmap2DROS cm( "xmlrpc", *tf_ ); + std::vector footprint = cm.getRobotFootprint(); + EXPECT_EQ( 4, footprint.size() ); + + EXPECT_EQ( 0.1f, footprint[ 0 ].x ); + EXPECT_EQ( 0.1f, footprint[ 0 ].y ); + EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + + EXPECT_EQ( -0.1f, footprint[ 1 ].x ); + EXPECT_EQ( 0.1f, footprint[ 1 ].y ); + EXPECT_EQ( 0.0f, footprint[ 1 ].z ); + + EXPECT_EQ( -0.1f, footprint[ 2 ].x ); + EXPECT_EQ( -0.1f, footprint[ 2 ].y ); + EXPECT_EQ( 0.0f, footprint[ 2 ].z ); + + EXPECT_EQ( 0.1f, footprint[ 3 ].x ); + EXPECT_EQ( -0.1f, footprint[ 3 ].y ); + EXPECT_EQ( 0.0f, footprint[ 3 ].z ); +} + +TEST( Costmap2DROS, footprint_from_same_level_param ) +{ + Costmap2DROS cm( "same_level", *tf_ ); + std::vector footprint = cm.getRobotFootprint(); + EXPECT_EQ( 3, footprint.size() ); + + EXPECT_EQ( 1.0f, footprint[ 0 ].x ); + EXPECT_EQ( 2.0f, footprint[ 0 ].y ); + EXPECT_EQ( 0.0f, footprint[ 0 ].z ); + + EXPECT_EQ( 3.0f, footprint[ 1 ].x ); + EXPECT_EQ( 4.0f, footprint[ 1 ].y ); + EXPECT_EQ( 0.0f, footprint[ 1 ].z ); + + EXPECT_EQ( 5.0f, footprint[ 2 ].x ); + EXPECT_EQ( 6.0f, footprint[ 2 ].y ); + EXPECT_EQ( 0.0f, footprint[ 2 ].z ); +} + +TEST( Costmap2DROS, footprint_from_xmlrpc_param_failure ) +{ + ASSERT_ANY_THROW( Costmap2DROS cm( "xmlrpc_fail", *tf_ )); +} + +TEST( Costmap2DROS, footprint_empty ) +{ + Costmap2DROS cm( "empty", *tf_ ); + std::vector footprint = cm.getRobotFootprint(); + // With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding. + EXPECT_EQ( 16, footprint.size() ); + + EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 ); + EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 ); + EXPECT_EQ( 0.0f, footprint[ 0 ].z ); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "footprint_tests_node"); + + tf_ = new tf::TransformListener( ros::Duration( 10 )); + + // This empty transform is added to satisfy the constructor of + // Costmap2DROS, which waits for the transform from map to base_link + // to become available. + tf::StampedTransform base_rel_map; + base_rel_map.child_frame_id_ = "/base_link"; + base_rel_map.frame_id_ = "/map"; + base_rel_map.stamp_ = ros::Time::now(); + base_rel_map.setIdentity(); + tf_->setTransform( base_rel_map ); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/libs/nav2_util/src/costmap_2d/test/footprint_tests.launch b/src/libs/nav2_util/src/costmap_2d/test/footprint_tests.launch new file mode 100644 index 0000000000..30edbdd4d2 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/footprint_tests.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + footprint_padding: 0 + footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]] + + + + footprint_padding: 0 + footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]] + + + + + + + + + diff --git a/src/libs/nav2_util/src/costmap_2d/test/inflation_tests.cpp b/src/libs/nav2_util/src/costmap_2d/test/inflation_tests.cpp new file mode 100644 index 0000000000..e4fdf98e9d --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/inflation_tests.cpp @@ -0,0 +1,411 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @author David Lu!! + * Test harness for InflationLayer for Costmap2D + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace costmap_2d; +using geometry_msgs::Point; + +std::vector setRadii(LayeredCostmap& layers, double length, double width, double inflation_radius) +{ + std::vector polygon; + Point p; + p.x = width; + p.y = length; + polygon.push_back(p); + p.x = width; + p.y = -length; + polygon.push_back(p); + p.x = -width; + p.y = -length; + polygon.push_back(p); + p.x = -width; + p.y = length; + polygon.push_back(p); + layers.setFootprint(polygon); + + ros::NodeHandle nh; + nh.setParam("/inflation_tests/inflation/inflation_radius", inflation_radius); + + return polygon; +} + +// Test that a single point gets inflated properly +void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap, InflationLayer* ilayer, double inflation_radius) +{ + bool* seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()]; + memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool)); + std::map > m; + CellData initial(costmap->getIndex(mx, my), mx, my, mx, my); + m[0].push_back(initial); + for (std::map >::iterator bin = m.begin(); bin != m.end(); ++bin) + { + for (int i = 0; i < bin->second.size(); ++i) + { + const CellData& cell = bin->second[i]; + if (!seen[cell.index_]) + { + seen[cell.index_] = true; + unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_; + unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_; + double dist = hypot(dx, dy); + + unsigned char expected_cost = ilayer->computeCost(dist); + ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost); + + if (dist > inflation_radius) + { + continue; + } + + if (cell.x_ > 0) + { + CellData data(costmap->getIndex(cell.x_-1, cell.y_), + cell.x_-1, cell.y_, cell.src_x_, cell.src_y_); + m[dist].push_back(data); + } + if (cell.y_ > 0) + { + CellData data(costmap->getIndex(cell.x_, cell.y_-1), + cell.x_, cell.y_-1, cell.src_x_, cell.src_y_); + m[dist].push_back(data); + } + if (cell.x_ < costmap->getSizeInCellsX() - 1) + { + CellData data(costmap->getIndex(cell.x_+1, cell.y_), + cell.x_+1, cell.y_, cell.src_x_, cell.src_y_); + m[dist].push_back(data); + } + if (cell.y_ < costmap->getSizeInCellsY() - 1) + { + CellData data(costmap->getIndex(cell.x_, cell.y_+1), + cell.x_, cell.y_+1, cell.src_x_, cell.src_y_); + m[dist].push_back(data); + } + } + } + } + delete[] seen; +} + +TEST(costmap, testAdjacentToObstacleCanStillMove){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + // Footprint with inscribed radius = 2.1 + // circumscribed radius = 3.1 + std::vector polygon = setRadii(layers, 2.1, 2.3, 4.1); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + InflationLayer* ilayer = addInflationLayer(layers, tf); + layers.setFootprint(polygon); + + addObservation(olayer, 0, 0, MAX_Z); + + layers.updateMap(0,0,0); + Costmap2D* costmap = layers.getCostmap(); + //printMap(*costmap); + EXPECT_EQ( LETHAL_OBSTACLE, costmap->getCost( 0, 0 )); + EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 )); + EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 )); + EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 )); + EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 )); + EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 )); +} + +TEST(costmap, testInflationShouldNotCreateUnknowns){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + // Footprint with inscribed radius = 2.1 + // circumscribed radius = 3.1 + std::vector polygon = setRadii(layers, 2.1, 2.3, 4.1); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + InflationLayer* ilayer = addInflationLayer(layers, tf); + layers.setFootprint(polygon); + + addObservation(olayer, 0, 0, MAX_Z); + + layers.updateMap(0,0,0); + Costmap2D* costmap = layers.getCostmap(); + + EXPECT_EQ( countValues(*costmap, NO_INFORMATION), 0 ); +} + + +/** + * Test for the cost function correctness with a larger range and different values + */ +TEST(costmap, testCostFunctionCorrectness){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + layers.resizeMap(100, 100, 1, 0, 0); + + // Footprint with inscribed radius = 5.0 + // circumscribed radius = 8.0 + std::vector polygon = setRadii(layers, 5.0, 6.25, 10.5); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + InflationLayer* ilayer = addInflationLayer(layers, tf); + layers.setFootprint(polygon); + + addObservation(olayer, 50, 50, MAX_Z); + + layers.updateMap(0,0,0); + Costmap2D* map = layers.getCostmap(); + + // Verify that the circumscribed cost lower bound is as expected: based on the cost function. + //unsigned char c = ilayer->computeCost(8.0); + //ASSERT_EQ(ilayer->getCircumscribedCost(), c); + + for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){ + // To the right + ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + // To the left + ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + // Down + ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + // Up + ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + } + + // Verify the normalized cost attenuates as expected + for(unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++){ + unsigned char expectedValue = ilayer->computeCost(i/1.0); + ASSERT_EQ(map->getCost(50 + i, 50), expectedValue); + } + + // Update with no hits. Should clear (revert to the static map + /*map->resetMapOutsideWindow(0, 0, 0.0, 0.0); + cloud.points.resize(0); + + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs2(p, cloud, 100.0, 100.0); + std::vector obsBuf2; + obsBuf2.push_back(obs2); + + map->updateWorld(0, 0, obsBuf2, obsBuf2); + + for(unsigned int i = 0; i < 100; i++) + for(unsigned int j = 0; j < 100; j++) + ASSERT_EQ(map->getCost(i, j), costmap_2d::FREE_SPACE);*/ +} + +/** + * Test that there is no regression and that costs do not get + * underestimated with the distance-as-key map used to replace + * the previously used priority queue. This is a more thorough + * test of the cost function being correctly applied. + */ +TEST(costmap, testInflationOrderCorrectness){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + // Footprint with inscribed radius = 2.1 + // circumscribed radius = 3.1 + const double inflation_radius = 4.1; + std::vector polygon = setRadii(layers, 2.1, 2.3, inflation_radius); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + InflationLayer* ilayer = addInflationLayer(layers, tf); + layers.setFootprint(polygon); + + // Add two diagonal cells, they would induce problems under the + // previous implementations + addObservation(olayer, 4, 4, MAX_Z); + addObservation(olayer, 5, 5, MAX_Z); + + layers.updateMap(0, 0, 0); + + validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius); + validatePointInflation(5, 5, layers.getCostmap(), ilayer, inflation_radius); +} + +/** + * Test inflation for both static and dynamic obstacles + */ +TEST(costmap, testInflation){ + + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + + // Footprint with inscribed radius = 2.1 + // circumscribed radius = 3.1 + std::vector polygon = setRadii(layers, 1, 1, 1); + + addStaticLayer(layers, tf); + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + InflationLayer* ilayer = addInflationLayer(layers, tf); + layers.setFootprint(polygon); + + Costmap2D* costmap = layers.getCostmap(); + + layers.updateMap(0,0,0); + //printMap(*costmap); + ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)20); + ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)28); + + /*/ Iterate over all id's and verify they are obstacles + for(std::vector::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){ + unsigned int ind = *it; + unsigned int x, y; + map.indexToCells(ind, x, y); + ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true); + ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + }*/ + + addObservation(olayer, 0, 0, 0.4); + layers.updateMap(0,0,0); + + // It and its 2 neighbors makes 3 obstacles + ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51); + + // @todo Rewrite + // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells + addObservation(olayer, 2, 0); + layers.updateMap(0,0,0); + + // Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from + // the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle + // at <0, 1> + ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)54); + + // Add an obstacle at <1, 9>. This will inflate obstacles around it + addObservation(olayer, 1, 9); + layers.updateMap(0,0,0); + + ASSERT_EQ(costmap->getCost(1, 9), LETHAL_OBSTACLE); + ASSERT_EQ(costmap->getCost(0, 9), INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(costmap->getCost(2, 9), INSCRIBED_INFLATED_OBSTACLE); + + // Add an obstacle and verify that it over-writes its inflated status + addObservation(olayer, 0, 9); + layers.updateMap(0,0,0); + + ASSERT_EQ(costmap->getCost(0, 9), LETHAL_OBSTACLE); +} + +/** + * Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles. + */ +TEST(costmap, testInflation2){ + + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + + // Footprint with inscribed radius = 2.1 + // circumscribed radius = 3.1 + std::vector polygon = setRadii(layers, 1, 1, 1); + + addStaticLayer(layers, tf); + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + InflationLayer* ilayer = addInflationLayer(layers, tf); + layers.setFootprint(polygon); + + // Creat a small L-Shape all at once + addObservation(olayer, 1, 1, MAX_Z); + addObservation(olayer, 2, 1, MAX_Z); + addObservation(olayer, 2, 2, MAX_Z); + layers.updateMap(0,0,0); + + Costmap2D* costmap = layers.getCostmap(); + //printMap(*costmap); + ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); +} + +/** + * Test inflation behavior, starting with an empty map + */ +TEST(costmap, testInflation3){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + // 1 2 3 + std::vector polygon = setRadii(layers, 1, 1.75, 3); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + InflationLayer* ilayer = addInflationLayer(layers, tf); + layers.setFootprint(polygon); + + // There should be no occupied cells + Costmap2D* costmap = layers.getCostmap(); + ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)0); + ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)0); + printMap(*costmap); + // Add an obstacle at 5,5 + addObservation(olayer, 5, 5, MAX_Z); + layers.updateMap(0,0,0); + printMap(*costmap); + + // Test fails because updated cell value is 0 + ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29); + ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1); + ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4); + + // Update again - should see no change + layers.updateMap(0,0,0); + + ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29); + ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1); + ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4); +} + + +int main(int argc, char** argv){ + ros::init(argc, argv, "inflation_tests"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/libs/nav2_util/src/costmap_2d/test/inflation_tests.launch b/src/libs/nav2_util/src/costmap_2d/test/inflation_tests.launch new file mode 100644 index 0000000000..1c2b2fe731 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/inflation_tests.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/libs/nav2_util/src/costmap_2d/test/module_tests.cpp b/src/libs/nav2_util/src/costmap_2d/test/module_tests.cpp new file mode 100644 index 0000000000..e011d3d9b5 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/module_tests.cpp @@ -0,0 +1,1177 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @author Conor McGann + * Test harness for Costmap2D + */ + +#include +#include +#include +#include +#include + +using namespace costmap_2d; + +const unsigned char MAP_10_BY_10_CHAR[] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 200, 200, 200, + 0, 0, 0, 0, 100, 0, 0, 200, 200, 200, + 0, 0, 0, 0, 100, 0, 0, 200, 200, 200, + 70, 70, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 200, 200, 200, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, + 0, 0, 0, 0, 0, 0, 0, 255, 255, 255 +}; + +const unsigned char MAP_5_BY_5_CHAR[] = { + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, +}; + +std::vector MAP_5_BY_5; +std::vector MAP_10_BY_10; +std::vector EMPTY_10_BY_10; +std::vector EMPTY_100_BY_100; + +const unsigned int GRID_WIDTH(10); +const unsigned int GRID_HEIGHT(10); +const double RESOLUTION(1); +const double WINDOW_LENGTH(10); +const unsigned char THRESHOLD(100); +const double MAX_Z(1.0); +const double RAYTRACE_RANGE(20.0); +const double OBSTACLE_RANGE(20.0); +const double ROBOT_RADIUS(1.0); + +bool find(const std::vector& l, unsigned int n){ + for(std::vector::const_iterator it = l.begin(); it != l.end(); ++it){ + if(*it == n) + return true; + } + + return false; +} + +/** + * Tests the reset method + */ +TEST(costmap, testResetForStaticMap){ + // Define a static map with a large object in the center + std::vector staticMap; + for(unsigned int i=0; i<10; i++){ + for(unsigned int j=0; j<10; j++){ + staticMap.push_back(costmap_2d::LETHAL_OBSTACLE); + } + } + + // Allocate the cost map, with a inflation to 3 cells all around + Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD); + + // Populate the cost map with a wall around the perimeter. Free space should clear out the room. + pcl::PointCloud cloud; + cloud.points.resize(40); + + // Left wall + unsigned int ind = 0; + for (unsigned int i = 0; i < 10; i++){ + // Left + cloud.points[ind].x = 0; + cloud.points[ind].y = i; + cloud.points[ind].z = MAX_Z; + ind++; + + // Top + cloud.points[ind].x = i; + cloud.points[ind].y = 0; + cloud.points[ind].z = MAX_Z; + ind++; + + // Right + cloud.points[ind].x = 9; + cloud.points[ind].y = i; + cloud.points[ind].z = MAX_Z; + ind++; + + // Bottom + cloud.points[ind].x = i; + cloud.points[ind].y = 9; + cloud.points[ind].z = MAX_Z; + ind++; + } + + double wx = 5.0, wy = 5.0; + geometry_msgs::Point p; + p.x = wx; + p.y = wy; + p.z = MAX_Z; + Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE); + std::vector obsBuf; + obsBuf.push_back(obs); + + // Update the cost map for this observation + map.updateWorld(wx, wy, obsBuf, obsBuf); + + // Verify that we now have only 36 cells with lethal cost, thus provong that we have correctly cleared + // free space + int hitCount = 0; + for(unsigned int i=0; i < 10; ++i){ + for(unsigned int j=0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + hitCount++; + } + } + } + ASSERT_EQ(hitCount, 36); + + // Veriy that we have 64 non-leathal + hitCount = 0; + for(unsigned int i=0; i < 10; ++i){ + for(unsigned int j=0; j < 10; ++j){ + if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE) + hitCount++; + } + } + ASSERT_EQ(hitCount, 64); + + // Now if we reset the cost map, we should have our map go back to being completely occupied + map.resetMapOutsideWindow(wx, wy, 0.0, 0.0); + + //We should now go back to everything being occupied + hitCount = 0; + for(unsigned int i=0; i < 10; ++i){ + for(unsigned int j=0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) + hitCount++; + } + } + ASSERT_EQ(hitCount, 100); + +} + +/** + * Test for the cost function correctness with a larger range and different values + */ +TEST(costmap, testCostFunctionCorrectness){ + Costmap2D map(100, 100, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS * 5.0, ROBOT_RADIUS * 8.0, ROBOT_RADIUS * 10.5, + 100.0, MAX_Z, 100.0, 25, EMPTY_100_BY_100, THRESHOLD); + + // Verify that the circumscribed cost lower bound is as expected: based on the cost function. + unsigned char c = map.computeCost((ROBOT_RADIUS * 8.0 / RESOLUTION)); + ASSERT_EQ(map.getCircumscribedCost(), c); + + // Add a point in the center + pcl::PointCloud cloud; + cloud.points.resize(1); + cloud.points[0].x = 50; + cloud.points[0].y = 50; + cloud.points[0].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, cloud, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + for(unsigned int i = 0; i <= (unsigned int)ceil(ROBOT_RADIUS * 5.0); i++){ + // To the right + ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + // To the left + ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + // Down + ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + // Up + ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + } + + // Verify the normalized cost attenuates as expected + for(unsigned int i = (unsigned int)(ceil(ROBOT_RADIUS * 5.0) + 1); i <= (unsigned int)ceil(ROBOT_RADIUS * 10.5); i++){ + unsigned char expectedValue = map.computeCost(i / RESOLUTION); + ASSERT_EQ(map.getCost(50 + i, 50), expectedValue); + } + + // Update with no hits. Should clear (revert to the static map + map.resetMapOutsideWindow(0, 0, 0.0, 0.0); + cloud.points.resize(0); + + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs2(p, cloud, 100.0, 100.0); + std::vector obsBuf2; + obsBuf2.push_back(obs2); + + map.updateWorld(0, 0, obsBuf2, obsBuf2); + + for(unsigned int i = 0; i < 100; i++) + for(unsigned int j = 0; j < 100; j++) + ASSERT_EQ(map.getCost(i, j), costmap_2d::FREE_SPACE); +} + +char printableCost( unsigned char cost ) +{ + switch( cost ) + { + case NO_INFORMATION: return '?'; + case LETHAL_OBSTACLE: return 'L'; + case INSCRIBED_INFLATED_OBSTACLE: return 'I'; + case FREE_SPACE: return '.'; + default: return '0' + (unsigned char) (10 * cost / 255); + } +} + +/** + * Test for wave interference + */ +TEST(costmap, testWaveInterference){ + // Start with an empty map + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3.01, + 10.0, MAX_Z * 2, 10.0, 1, EMPTY_10_BY_10, THRESHOLD); + + // Lay out 3 obstacles in a line - along the diagonal, separated by a cell. + pcl::PointCloud cloud; + cloud.points.resize(3); + cloud.points[0].x = 3; + cloud.points[0].y = 3; + cloud.points[0].z = MAX_Z; + cloud.points[1].x = 5; + cloud.points[1].y = 5; + cloud.points[1].z = MAX_Z; + cloud.points[2].x = 7; + cloud.points[2].y = 7; + cloud.points[2].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, cloud, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + int update_count = 0; + + // Expect to see a union of obstacles + printf("map:\n"); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) != costmap_2d::FREE_SPACE){ + update_count++; + } + printf("%c", printableCost( map.getCost( i, j ))); + } + printf("\n"); + } + + ASSERT_EQ(update_count, 79); +} + +/** Test for copying a window of a costmap */ +TEST(costmap, testWindowCopy){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + /* + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + printf("%3d ", map.getCost(i, j)); + } + printf("\n"); + } + printf("\n"); + */ + + Costmap2D windowCopy; + + //first test that if we try to make a window that is too big, things fail + windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0); + ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0); + ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0); + + //Next, test that trying to make a map window itself fails + map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0); + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10); + + //Next, test that legal input generates the result that we expect + windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0); + ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6); + ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6); + + //check that we actually get the windo that we expect + for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){ + for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){ + ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2)); + //printf("%3d ", windowCopy.getCost(i, j)); + } + //printf("\n"); + } + +} + +//test for updating costmaps with static data +TEST(costmap, testFullyContainedStaticMapUpdate){ + Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD); + + Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + map.updateStaticMapWindow(0, 0, 10, 10, MAP_10_BY_10); + + for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ + for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ + ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); + } + } +} + +TEST(costmap, testOverlapStaticMapUpdate){ + Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD); + + Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + map.updateStaticMapWindow(-10, -10, 10, 10, MAP_10_BY_10); + + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); + } + } + + std::vector blank(100); + + //check to make sure that inflation on updates are being done correctly + map.updateStaticMapWindow(-10, -10, 10, 10, blank); + + for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ + for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ + ASSERT_EQ(map.getCost(i, j), 0); + } + } + + std::vector fully_contained(25); + fully_contained[0] = 254; + fully_contained[4] = 254; + fully_contained[5] = 254; + fully_contained[9] = 254; + + Costmap2D small_static_map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD); + + map.updateStaticMapWindow(0, 0, 5, 5, fully_contained); + + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); + for(unsigned int j = 0; j < 5; ++j){ + for(unsigned int i = 0; i < 5; ++i){ + ASSERT_EQ(map.getCost(i + 10, j + 10), small_static_map.getCost(i, j)); + } + } + +} + +/** + * Test for ray tracing free space + */ +TEST(costmap, testRaytracing){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + // Add a point cloud, should not affect the map + pcl::PointCloud cloud; + cloud.points.resize(1); + cloud.points[0].x = 0; + cloud.points[0].y = 0; + cloud.points[0].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, cloud, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + int lethal_count = 0; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + lethal_count++; + } + } + } + + //we expect just one obstacle to be added + ASSERT_EQ(lethal_count, 21); +} + +TEST(costmap, testAdjacentToObstacleCanStillMove){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + pcl::PointCloud cloud; + cloud.points.resize(1); + cloud.points[0].x = 0; + cloud.points[0].y = 0; + cloud.points[0].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, cloud, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(9, 9, obsBuf, obsBuf); + + EXPECT_EQ( LETHAL_OBSTACLE, map.getCost( 0, 0 )); + EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 0 )); + EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 2, 0 )); + EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > map.getCost( 3, 0 )); + EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > map.getCost( 2, 1 )); + EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 1 )); +} + +TEST(costmap, testInflationShouldNotCreateUnknowns){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + pcl::PointCloud cloud; + cloud.points.resize(1); + cloud.points[0].x = 0; + cloud.points[0].y = 0; + cloud.points[0].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, cloud, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(9, 9, obsBuf, obsBuf); + + int unknown_count = 0; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::NO_INFORMATION){ + unknown_count++; + } + } + } + EXPECT_EQ( 0, unknown_count ); +} + +unsigned int worldToIndex(Costmap2D& map, double wx, double wy){ + unsigned int mx, my; + map.worldToMap(wx, wy, mx, my); + return map.getIndex(mx, my); +} + +void indexToWorld(Costmap2D& map, unsigned int index, double& wx, double& wy){ + unsigned int mx, my; + map.indexToCells(index, mx, my); + map.mapToWorld(mx, my, wx, wy); +} + +TEST(costmap, testStaticMap){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10); + + // Verify that obstacles correctly identified from the static map. + std::vector occupiedCells; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + occupiedCells.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(occupiedCells.size(), (unsigned int)20); + + // Iterate over all id's and verify that they are present according to their + for(std::vector::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){ + unsigned int ind = *it; + unsigned int x, y; + map.indexToCells(ind, x, y); + ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true); + ASSERT_EQ(MAP_10_BY_10[ind] >= 100, true); + ASSERT_EQ(map.getCost(x, y) >= 100, true); + } + + // Block of 200 + ASSERT_EQ(find(occupiedCells, map.getIndex(7, 2)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(8, 2)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(9, 2)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(7, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(8, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(9, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(7, 4)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(8, 4)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(9, 4)), true); + + // Block of 100 + ASSERT_EQ(find(occupiedCells, map.getIndex(4, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(4, 4)), true); + + // Block of 200 + ASSERT_EQ(find(occupiedCells, map.getIndex(3, 7)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(4, 7)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(5, 7)), true); + + + // Verify Coordinate Transformations, ROW MAJOR ORDER + ASSERT_EQ(worldToIndex(map, 0.0, 0.0), (unsigned int)0); + ASSERT_EQ(worldToIndex(map, 0.0, 0.99), (unsigned int)0); + ASSERT_EQ(worldToIndex(map, 0.0, 1.0), (unsigned int)10); + ASSERT_EQ(worldToIndex(map, 1.0, 0.99), (unsigned int)1); + ASSERT_EQ(worldToIndex(map, 9.99, 9.99), (unsigned int)99); + ASSERT_EQ(worldToIndex(map, 8.2, 3.4), (unsigned int)38); + + // Ensure we hit the middle of the cell for world co-ordinates + double wx, wy; + indexToWorld(map, 99, wx, wy); + ASSERT_EQ(wx, 9.5); + ASSERT_EQ(wy, 9.5); +} + + +/** + * Verify that dynamic obstacles are added + */ + +TEST(costmap, testDynamicObstacles){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + // Add a point cloud and verify its insertion. There should be only one new one + pcl::PointCloud cloud; + cloud.points.resize(3); + cloud.points[0].x = 0; + cloud.points[0].y = 0; + cloud.points[1].x = 0; + cloud.points[1].y = 0; + cloud.points[2].x = 0; + cloud.points[2].y = 0; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, cloud, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + std::vector ids; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + ids.push_back(map.getIndex(i, j)); + } + } + } + + // Should now have 1 insertion and no deletions + ASSERT_EQ(ids.size(), (unsigned int)21); + + // Repeating the call - we should see no insertions or deletions + map.updateWorld(0, 0, obsBuf, obsBuf); + ASSERT_EQ(ids.size(), (unsigned int)21); +} + +/** + * Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle + */ +TEST(costmap, testMultipleAdditions){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + // A point cloud with one point that falls within an existing obstacle + pcl::PointCloud cloud; + cloud.points.resize(1); + cloud.points[0].x = 7; + cloud.points[0].y = 2; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, cloud, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + std::vector ids; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + ids.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(ids.size(), (unsigned int)20); +} + +/** + * Make sure we ignore points outside of our z threshold + */ +TEST(costmap, testZThreshold){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + // A point cloud with 2 points falling in a cell with a non-lethal cost + pcl::PointCloud c0; + c0.points.resize(2); + c0.points[0].x = 0; + c0.points[0].y = 5; + c0.points[0].z = 0.4; + c0.points[1].x = 1; + c0.points[1].y = 5; + c0.points[1].z = 1.2; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, c0, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + std::vector ids; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + ids.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(ids.size(), (unsigned int)21); +} + +/** + * Test inflation for both static and dynamic obstacles + */ + +TEST(costmap, testInflation){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + // Verify that obstacles correctly identified + std::vector occupiedCells; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + occupiedCells.push_back(map.getIndex(i, j)); + } + } + } + + // There should be no duplicates + std::set setOfCells; + for(unsigned int i=0;i::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){ + unsigned int ind = *it; + unsigned int x, y; + map.indexToCells(ind, x, y); + ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true); + ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + } + + // Set an obstacle at the origin and observe insertions for it and its neighbors + pcl::PointCloud c0; + c0.points.resize(1); + c0.points[0].x = 0; + c0.points[0].y = 0; + c0.points[0].z = 0.4; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, c0, 100.0, 100.0); + std::vector obsBuf, empty; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, empty); + + occupiedCells.clear(); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + occupiedCells.push_back(map.getIndex(i, j)); + } + } + } + + // It and its 2 neighbors makes 3 obstacles + ASSERT_EQ(occupiedCells.size(), (unsigned int)51); + + // @todo Rewrite + // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells + pcl::PointCloud c1; + c1.points.resize(1); + c1.points[0].x = 2; + c1.points[0].y = 0; + c1.points[0].z = 0.0; + + geometry_msgs::Point p1; + p1.x = 0.0; + p1.y = 0.0; + p1.z = MAX_Z; + + Observation obs1(p1, c1, 100.0, 100.0); + std::vector obsBuf1; + obsBuf1.push_back(obs1); + + map.updateWorld(0, 0, obsBuf1, empty); + + occupiedCells.clear(); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + occupiedCells.push_back(map.getIndex(i, j)); + } + } + } + + // Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from + // the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle + // at <0, 1> + ASSERT_EQ(occupiedCells.size(), (unsigned int)54); + + + // Add an obstacle at <1, 9>. This will inflate obstacles around it + pcl::PointCloud c2; + c2.points.resize(1); + c2.points[0].x = 1; + c2.points[0].y = 9; + c2.points[0].z = 0.0; + + geometry_msgs::Point p2; + p2.x = 0.0; + p2.y = 0.0; + p2.z = MAX_Z; + + Observation obs2(p2, c2, 100.0, 100.0); + std::vector obsBuf2; + obsBuf2.push_back(obs2); + + map.updateWorld(0, 0, obsBuf2, empty); + + ASSERT_EQ(map.getCost(1, 9), costmap_2d::LETHAL_OBSTACLE); + ASSERT_EQ(map.getCost(0, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(map.getCost(2, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + + // Add an obstacle and verify that it over-writes its inflated status + pcl::PointCloud c3; + c3.points.resize(1); + c3.points[0].x = 0; + c3.points[0].y = 9; + c3.points[0].z = 0.0; + + geometry_msgs::Point p3; + p3.x = 0.0; + p3.y = 0.0; + p3.z = MAX_Z; + + Observation obs3(p3, c3, 100.0, 100.0); + std::vector obsBuf3; + obsBuf3.push_back(obs3); + + map.updateWorld(0, 0, obsBuf3, empty); + + ASSERT_EQ(map.getCost(0, 9), costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles. + */ +TEST(costmap, testInflation2){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + // Creat a small L-Shape all at once + pcl::PointCloud c0; + c0.points.resize(3); + c0.points[0].x = 1; + c0.points[0].y = 1; + c0.points[0].z = MAX_Z; + c0.points[1].x = 1; + c0.points[1].y = 2; + c0.points[1].z = MAX_Z; + c0.points[2].x = 2; + c0.points[2].y = 2; + c0.points[2].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, c0, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + ASSERT_EQ(map.getCost(3, 2), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(map.getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); +} + +/** + * Test inflation behavior, starting with an empty map + */ +TEST(costmap, testInflation3){ + std::vector mapData; + for(unsigned int i=0; i< GRID_WIDTH; i++){ + for(unsigned int j = 0; j < GRID_HEIGHT; j++){ + mapData.push_back(0); + } + } + + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3, + 10.0, MAX_Z, 10.0, 1, mapData, THRESHOLD); + + // There should be no occupied cells + std::vector ids; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + ids.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(ids.size(), (unsigned int)0); + + // Add an obstacle at 5,5 + pcl::PointCloud c0; + c0.points.resize(1); + c0.points[0].x = 5; + c0.points[0].y = 5; + c0.points[0].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.0; + p.y = 0.0; + p.z = MAX_Z; + + Observation obs(p, c0, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) != costmap_2d::FREE_SPACE){ + ids.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(ids.size(), (unsigned int)29); + + ids.clear(); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + ids.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(ids.size(), (unsigned int)5); + + // Update again - should see no change + map.updateWorld(0, 0, obsBuf, obsBuf); + + ids.clear(); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) != costmap_2d::FREE_SPACE){ + ids.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(ids.size(), (unsigned int)29); +} + +/** + * Test for ray tracing free space + */ + +TEST(costmap, testRaytracing2){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 100.0, MAX_Z, 100.0, 1, MAP_10_BY_10, THRESHOLD); + + // The sensor origin will be <0,0>. So if we add an obstacle at 9,9, we would expect cells + // <0, 0> thru <8, 8> to be traced through + pcl::PointCloud c0; + c0.points.resize(1); + c0.points[0].x = 9.5; + c0.points[0].y = 9.5; + c0.points[0].z = MAX_Z; + + geometry_msgs::Point p; + p.x = 0.5; + p.y = 0.5; + p.z = MAX_Z; + + Observation obs(p, c0, 100.0, 100.0); + std::vector obsBuf; + obsBuf.push_back(obs); + + std::vector obstacles; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + obstacles.push_back(map.getIndex(i, j)); + } + } + } + + unsigned int obs_before = obstacles.size(); + + map.updateWorld(0, 0, obsBuf, obsBuf); + + obstacles.clear(); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + obstacles.push_back(map.getIndex(i, j)); + } + } + } + + //Two obstacles shoulb be removed from the map by raytracing + ASSERT_EQ(obstacles.size(), obs_before - 2); + + + // many cells will have been switched to free space along the diagonal except + // for those inflated in the update.. tests that inflation happens properly + // after raytracing + unsigned char test[10]= {0, 0, 0, 253, 253, 0, 0, 253, 253, 254}; + for(unsigned int i=0; i < 10; i++) + ASSERT_EQ(map.getCost(i, i), test[i]); +} + +/** + * Within a certian radius of the robot, the cost map most propagate obstacles. This + * is to avoid a case where a hit on a far obstacle clears inscribed radius around a + * near one. + */ + +TEST(costmap, testTrickyPropagation){ + const unsigned char MAP_HALL_CHAR[10 * 10] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 254, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 254, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + }; + std::vector MAP_HALL; + for (int i = 0; i < 10 * 10; i++) { + MAP_HALL.push_back(MAP_HALL_CHAR[i]); + } + + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 100.0, MAX_Z, 100.0, 1, MAP_HALL, THRESHOLD); + + + //Add a dynamic obstacle + pcl::PointCloud c2; + c2.points.resize(3); + //Dynamic obstacle that raytaces. + c2.points[0].x = 7.0; + c2.points[0].y = 8.0; + c2.points[0].z = 1.0; + //Dynamic obstacle that should not be raytraced the + //first update, but should on the second. + c2.points[1].x = 3.0; + c2.points[1].y = 4.0; + c2.points[1].z = 1.0; + //Dynamic obstacle that should not be erased. + c2.points[2].x = 6.0; + c2.points[2].y = 3.0; + c2.points[2].z = 1.0; + + geometry_msgs::Point p2; + p2.x = 0.5; + p2.y = 0.5; + p2.z = MAX_Z; + + Observation obs2(p2, c2, 100.0, 100.0); + std::vector obsBuf2; + obsBuf2.push_back(obs2); + + map.updateWorld(0, 0, obsBuf2, obsBuf2); + + const unsigned char MAP_HALL_CHAR_TEST[10 * 10] = { + 253, 254, 253, 0, 0, 0, 0, 0, 0, 0, + 0, 253, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 253, 0, 0, 0, 0, 0, + 0, 0, 0, 253, 254, 253, 0, 0, 0, 0, + 0, 0, 0, 0, 253, 0, 0, 253, 0, 0, + 0, 0, 0, 253, 0, 0, 253, 254, 253, 0, + 0, 0, 253, 254, 253, 0, 0, 253, 253, 0, + 0, 0, 0, 253, 0, 0, 0, 253, 254, 253, + 0, 0, 0, 0, 0, 0, 0, 0, 253, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + }; + + + for (int i = 0; i < 10 * 10; i++) { + ASSERT_EQ(map.getCost(i / 10, i % 10), MAP_HALL_CHAR_TEST[i]); + } + + pcl::PointCloud c; + c.points.resize(1); + //Dynamic obstacle that raytaces the one at (3.0, 4.0). + c.points[0].x = 4.0; + c.points[0].y = 5.0; + c.points[0].z = 1.0; + + geometry_msgs::Point p3; + p3.x = 0.5; + p3.y = 0.5; + p3.z = MAX_Z; + + Observation obs3(p3, c, 100.0, 100.0); + std::vector obsBuf3; + obsBuf3.push_back(obs3); + + map.updateWorld(0, 0, obsBuf3, obsBuf3); + + const unsigned char MAP_HALL_CHAR_TEST2[10 * 10] = { + 253, 254, 253, 0, 0, 0, 0, 0, 0, 0, + 0, 253, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 253, 0, 0, 0, 0, + 0, 0, 0, 0, 253, 254, 253, 253, 0, 0, + 0, 0, 0, 253, 0, 253, 253, 254, 253, 0, + 0, 0, 253, 254, 253, 0, 0, 253, 253, 0, + 0, 0, 0, 253, 0, 0, 0, 253, 254, 253, + 0, 0, 0, 0, 0, 0, 0, 0, 253, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + }; + + + for (int i = 0; i < 10 * 10; i++) { + ASSERT_EQ(map.getCost(i / 10, i % 10), MAP_HALL_CHAR_TEST2[i]); + } +} + + + +int main(int argc, char** argv){ + for(unsigned int i = 0; i< GRID_WIDTH * GRID_HEIGHT; i++){ + EMPTY_10_BY_10.push_back(0); + MAP_10_BY_10.push_back(MAP_10_BY_10_CHAR[i]); + } + + for(unsigned int i = 0; i< 5 * 5; i++){ + MAP_5_BY_5.push_back(MAP_10_BY_10_CHAR[i]); + } + + for(unsigned int i = 0; i< 100 * 100; i++) + EMPTY_100_BY_100.push_back(0); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.cpp b/src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.cpp new file mode 100644 index 0000000000..01b3d552f1 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.cpp @@ -0,0 +1,252 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @author David Lu!! + * Test harness for ObstacleLayer for Costmap2D + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace costmap_2d; + +/* + * For reference, the static map looks like this: + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 254 254 254 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 254 0 0 254 254 254 + * + * 0 0 0 0 254 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 254 254 254 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * 0 0 0 0 0 0 0 0 0 0 + * + * upper left is 0,0, lower right is 9,9 + */ + +/** + * Test for ray tracing free space + */ +TEST(costmap, testRaytracing){ + tf::TransformListener tf; + + LayeredCostmap layers("frame", false, false); // Not rolling window, not tracking unknown + addStaticLayer(layers, tf); // This adds the static map + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + + // Add a point at 0, 0, 0 + addObservation(olayer, 0.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2); + + // This actually puts the LETHAL (254) point in the costmap at (0,0) + layers.updateMap(0,0,0); // 0, 0, 0 is robot pose + //printMap(*(layers.getCostmap())); + + int lethal_count = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE); + + // We expect just one obstacle to be added (20 in static map) + ASSERT_EQ(lethal_count, 21); +} + +/** + * Test for ray tracing free space + */ +TEST(costmap, testRaytracing2){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + addStaticLayer(layers, tf); + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + + //If we print map now, it is 10x10 all value 0 + //printMap(*(layers.getCostmap())); + + // Update will fill in the costmap with the static map + layers.updateMap(0,0,0); + + //If we print the map now, we get the static map + //printMap(*(layers.getCostmap())); + + // Static map has 20 LETHAL cells (see diagram above) + int obs_before = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE); + ASSERT_EQ(obs_before, 20); + + // The sensor origin will be <0,0>. So if we add an obstacle at 9,9, + // we would expect cells <0, 0> thru <8, 8> to be traced through + // however the static map is not cleared by obstacle layer + addObservation(olayer, 9.5, 9.5, MAX_Z/2, 0.5, 0.5, MAX_Z/2); + layers.updateMap(0, 0, 0); + + // If we print map now, we have static map + <9,9> is LETHAL + //printMap(*(layers.getCostmap())); + int obs_after = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE); + + // Change from previous test: + // No obstacles from the static map will be cleared, so the + // net change is +1. + ASSERT_EQ(obs_after, obs_before + 1); + + // Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot + for(int i = 0; i < olayer->getSizeInCellsY(); ++i) + { + olayer->setCost(i, i, LETHAL_OBSTACLE); + } + // This will updateBounds, which will raytrace the static observation added + // above, thus clearing out the diagonal again! + layers.updateMap(0, 0, 0); + + // Map now has diagonal except <0,0> filled with LETHAL (254) + //printMap(*(layers.getCostmap())); + int with_static = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE); + + // Should thus be the same + ASSERT_EQ(with_static, obs_after); + // If 21 are filled, 79 should be free + ASSERT_EQ(79, countValues(*(layers.getCostmap()), costmap_2d::FREE_SPACE)); +} + +/** + * Test for wave interference + */ +TEST(costmap, testWaveInterference){ + tf::TransformListener tf; + + // Start with an empty map, no rolling window, tracking unknown + LayeredCostmap layers("frame", false, true); + layers.resizeMap(10, 10, 1, 0, 0); + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + + // If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION) + //printMap(*(layers.getCostmap())); + + // Lay out 3 obstacles in a line - along the diagonal, separated by a cell. + addObservation(olayer, 3.0, 3.0, MAX_Z); + addObservation(olayer, 5.0, 5.0, MAX_Z); + addObservation(olayer, 7.0, 7.0, MAX_Z); + layers.updateMap(0,0,0); + + Costmap2D* costmap = layers.getCostmap(); + // 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free + // <0,0> is footprint and is free + //printMap(*costmap); + ASSERT_EQ(3, countValues(*costmap, costmap_2d::LETHAL_OBSTACLE)); + ASSERT_EQ(92, countValues(*costmap, costmap_2d::NO_INFORMATION)); + ASSERT_EQ(5, countValues(*costmap, costmap_2d::FREE_SPACE)); +} + +/** + * Make sure we ignore points outside of our z threshold + */ +TEST(costmap, testZThreshold){ + tf::TransformListener tf; + // Start with an empty map + LayeredCostmap layers("frame", false, true); + layers.resizeMap(10, 10, 1, 0, 0); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + + // A point cloud with 2 points falling in a cell with a non-lethal cost + addObservation(olayer, 0.0, 5.0, 0.4); + addObservation(olayer, 1.0, 5.0, 2.2); + + layers.updateMap(0,0,0); + + Costmap2D* costmap = layers.getCostmap(); + ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 1); +} + + +/** + * Verify that dynamic obstacles are added + */ +TEST(costmap, testDynamicObstacles){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + addStaticLayer(layers, tf); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + + // Add a point cloud and verify its insertion. There should be only one new one + addObservation(olayer, 0.0, 0.0); + addObservation(olayer, 0.0, 0.0); + addObservation(olayer, 0.0, 0.0); + + layers.updateMap(0,0,0); + + Costmap2D* costmap = layers.getCostmap(); + // Should now have 1 insertion and no deletions + ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21); + + // Repeating the call - we should see no insertions or deletions + ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21); +} + + +/** + * Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle + */ +TEST(costmap, testMultipleAdditions){ + tf::TransformListener tf; + LayeredCostmap layers("frame", false, false); + addStaticLayer(layers, tf); + + ObstacleLayer* olayer = addObstacleLayer(layers, tf); + + // A point cloud with one point that falls within an existing obstacle + addObservation(olayer, 9.5, 0.0); + layers.updateMap(0,0,0); + Costmap2D* costmap = layers.getCostmap(); + //printMap(*costmap); + + ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 20); + +} + + +int main(int argc, char** argv){ + ros::init(argc, argv, "obstacle_tests"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.launch b/src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.launch new file mode 100644 index 0000000000..d94cfab200 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/obstacle_tests.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/src/libs/nav2_util/src/costmap_2d/test/simple_driving_test.xml b/src/libs/nav2_util/src/costmap_2d/test/simple_driving_test.xml new file mode 100644 index 0000000000..6bf507d273 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/simple_driving_test.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/src/libs/nav2_util/src/costmap_2d/test/static_tests.cpp b/src/libs/nav2_util/src/costmap_2d/test/static_tests.cpp new file mode 100644 index 0000000000..509933a5f8 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/static_tests.cpp @@ -0,0 +1,331 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @author David Lu!! + * Test harness for StaticMap Layer for Costmap2D + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace costmap_2d; + + +/** + * Tests the reset method + * +TEST(costmap, testResetForStaticMap){ + // Define a static map with a large object in the center + std::vector staticMap; + for(unsigned int i=0; i<10; i++){ + for(unsigned int j=0; j<10; j++){ + staticMap.push_back(costmap_2d::LETHAL_OBSTACLE); + } + } + + // Allocate the cost map, with a inflation to 3 cells all around + Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD); + + // Populate the cost map with a wall around the perimeter. Free space should clear out the room. + pcl::PointCloud cloud; + cloud.points.resize(40); + + // Left wall + unsigned int ind = 0; + for (unsigned int i = 0; i < 10; i++){ + // Left + cloud.points[ind].x = 0; + cloud.points[ind].y = i; + cloud.points[ind].z = MAX_Z; + ind++; + + // Top + cloud.points[ind].x = i; + cloud.points[ind].y = 0; + cloud.points[ind].z = MAX_Z; + ind++; + + // Right + cloud.points[ind].x = 9; + cloud.points[ind].y = i; + cloud.points[ind].z = MAX_Z; + ind++; + + // Bottom + cloud.points[ind].x = i; + cloud.points[ind].y = 9; + cloud.points[ind].z = MAX_Z; + ind++; + } + + double wx = 5.0, wy = 5.0; + geometry_msgs::Point p; + p.x = wx; + p.y = wy; + p.z = MAX_Z; + Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE); + std::vector obsBuf; + obsBuf.push_back(obs); + + // Update the cost map for this observation + map.updateWorld(wx, wy, obsBuf, obsBuf); + + // Verify that we now have only 36 cells with lethal cost, thus provong that we have correctly cleared + // free space + int hitCount = 0; + for(unsigned int i=0; i < 10; ++i){ + for(unsigned int j=0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + hitCount++; + } + } + } + ASSERT_EQ(hitCount, 36); + + // Veriy that we have 64 non-leathal + hitCount = 0; + for(unsigned int i=0; i < 10; ++i){ + for(unsigned int j=0; j < 10; ++j){ + if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE) + hitCount++; + } + } + ASSERT_EQ(hitCount, 64); + + // Now if we reset the cost map, we should have our map go back to being completely occupied + map.resetMapOutsideWindow(wx, wy, 0.0, 0.0); + + //We should now go back to everything being occupied + hitCount = 0; + for(unsigned int i=0; i < 10; ++i){ + for(unsigned int j=0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) + hitCount++; + } + } + ASSERT_EQ(hitCount, 100); + +} + +/** Test for copying a window of a costmap * +TEST(costmap, testWindowCopy){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + /* + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + printf("%3d ", map.getCost(i, j)); + } + printf("\n"); + } + printf("\n"); + * + + Costmap2D windowCopy; + + //first test that if we try to make a window that is too big, things fail + windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0); + ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0); + ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0); + + //Next, test that trying to make a map window itself fails + map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0); + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10); + + //Next, test that legal input generates the result that we expect + windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0); + ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6); + ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6); + + //check that we actually get the windo that we expect + for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){ + for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){ + ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2)); + //printf("%3d ", windowCopy.getCost(i, j)); + } + //printf("\n"); + } + +} + +//test for updating costmaps with static data +TEST(costmap, testFullyContainedStaticMapUpdate){ + Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD); + + Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + map.updateStaticMapWindow(0, 0, 10, 10, MAP_10_BY_10); + + for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ + for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ + ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); + } + } +} + +TEST(costmap, testOverlapStaticMapUpdate){ + Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD); + + Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + map.updateStaticMapWindow(-10, -10, 10, 10, MAP_10_BY_10); + + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); + } + } + + std::vector blank(100); + + //check to make sure that inflation on updates are being done correctly + map.updateStaticMapWindow(-10, -10, 10, 10, blank); + + for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ + for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ + ASSERT_EQ(map.getCost(i, j), 0); + } + } + + std::vector fully_contained(25); + fully_contained[0] = 254; + fully_contained[4] = 254; + fully_contained[5] = 254; + fully_contained[9] = 254; + + Costmap2D small_static_map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD); + + map.updateStaticMapWindow(0, 0, 5, 5, fully_contained); + + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_FLOAT_EQ(map.getOriginX(), -10); + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); + for(unsigned int j = 0; j < 5; ++j){ + for(unsigned int i = 0; i < 5; ++i){ + ASSERT_EQ(map.getCost(i + 10, j + 10), small_static_map.getCost(i, j)); + } + } + +} + + +TEST(costmap, testStaticMap){ + Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, + 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); + + ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10); + ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10); + + // Verify that obstacles correctly identified from the static map. + std::vector occupiedCells; + + for(unsigned int i = 0; i < 10; ++i){ + for(unsigned int j = 0; j < 10; ++j){ + if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + occupiedCells.push_back(map.getIndex(i, j)); + } + } + } + + ASSERT_EQ(occupiedCells.size(), (unsigned int)20); + + // Iterate over all id's and verify that they are present according to their + for(std::vector::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){ + unsigned int ind = *it; + unsigned int x, y; + map.indexToCells(ind, x, y); + ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true); + ASSERT_EQ(MAP_10_BY_10[ind] >= 100, true); + ASSERT_EQ(map.getCost(x, y) >= 100, true); + } + + // Block of 200 + ASSERT_EQ(find(occupiedCells, map.getIndex(7, 2)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(8, 2)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(9, 2)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(7, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(8, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(9, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(7, 4)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(8, 4)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(9, 4)), true); + + // Block of 100 + ASSERT_EQ(find(occupiedCells, map.getIndex(4, 3)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(4, 4)), true); + + // Block of 200 + ASSERT_EQ(find(occupiedCells, map.getIndex(3, 7)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(4, 7)), true); + ASSERT_EQ(find(occupiedCells, map.getIndex(5, 7)), true); + + + // Verify Coordinate Transformations, ROW MAJOR ORDER + ASSERT_EQ(worldToIndex(map, 0.0, 0.0), (unsigned int)0); + ASSERT_EQ(worldToIndex(map, 0.0, 0.99), (unsigned int)0); + ASSERT_EQ(worldToIndex(map, 0.0, 1.0), (unsigned int)10); + ASSERT_EQ(worldToIndex(map, 1.0, 0.99), (unsigned int)1); + ASSERT_EQ(worldToIndex(map, 9.99, 9.99), (unsigned int)99); + ASSERT_EQ(worldToIndex(map, 8.2, 3.4), (unsigned int)38); + + // Ensure we hit the middle of the cell for world co-ordinates + double wx, wy; + indexToWorld(map, 99, wx, wy); + ASSERT_EQ(wx, 9.5); + ASSERT_EQ(wy, 9.5); +} + +//*/ + + +int main(int argc, char** argv){ + ros::init(argc, argv, "obstacle_tests"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/libs/nav2_util/src/costmap_2d/test/static_tests.launch b/src/libs/nav2_util/src/costmap_2d/test/static_tests.launch new file mode 100644 index 0000000000..23a4ba9319 --- /dev/null +++ b/src/libs/nav2_util/src/costmap_2d/test/static_tests.launch @@ -0,0 +1,5 @@ + + + + + From aeccc47ca3aa6552c9c1bd0e9cee382798010b16 Mon Sep 17 00:00:00 2001 From: Brian Date: Fri, 14 Sep 2018 16:23:13 -0700 Subject: [PATCH 2/2] add COLCON_IGNORE --- src/libs/nav2_util/src/costmap_2d/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/libs/nav2_util/src/costmap_2d/COLCON_IGNORE diff --git a/src/libs/nav2_util/src/costmap_2d/COLCON_IGNORE b/src/libs/nav2_util/src/costmap_2d/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2