Add costmap_2d package sources
Convert navigations/costmap_2d from gitlink to normal tracked files.
This commit is contained in:
Submodule navigations/costmap_2d deleted from f97f0e7f3f
347
navigations/costmap_2d/CHANGELOG.rst
Executable file
347
navigations/costmap_2d/CHANGELOG.rst
Executable file
@@ -0,0 +1,347 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package costmap_2d
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
1.17.3 (2023-01-10)
|
||||
-------------------
|
||||
* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
|
||||
* do not specify obsolete c++11 standard
|
||||
this breaks with current versions of log4cxx.
|
||||
* update pluginlib include paths
|
||||
the non-hpp headers have been deprecated since kinetic
|
||||
* use lambdas in favor of boost::bind
|
||||
Using boost's _1 as a global system is deprecated since C++11.
|
||||
The ROS packages in Debian removed the implicit support for the global symbols,
|
||||
so this code fails to compile there without the patch.
|
||||
* Contributors: Michael Görner
|
||||
|
||||
1.17.2 (2022-06-20)
|
||||
-------------------
|
||||
* Removed unused variables in costmap_2d_ros (`#1126 <https://github.com/ros-planning/navigation/issues/1126>`_)
|
||||
remove unused `robot_stopped\_`, `old_pose\_` and `timer\_`
|
||||
* Check if stamp of transformed pose is non-zero (`#1200 <https://github.com/ros-planning/navigation/issues/1200>`_)
|
||||
* fix crashes in AMCL (`#1152 <https://github.com/ros-planning/navigation/issues/1152>`_)
|
||||
* fix: catch runtime_error from roscore
|
||||
* ignore malformed message from laser, otherwise it will crash
|
||||
* The main function has to return an int value. (`#1101 <https://github.com/ros-planning/navigation/issues/1101>`_)
|
||||
* fix boundary point exclusion in convexFillCells (`#1095 <https://github.com/ros-planning/navigation/issues/1095>`_)
|
||||
* Fix deadlock when starting map with map-update-frequency set to zero (`#1072 <https://github.com/ros-planning/navigation/issues/1072>`_)
|
||||
Co-authored-by: Dima Dorezyuk <dorezyuk@magazino.eu>
|
||||
* Increase scope of costmap mutex in publishCostmap to cover costmap calls (`#992 <https://github.com/ros-planning/navigation/issues/992>`_)
|
||||
* costmap_2d: remove useless and buggy testing helper function (`#1069 <https://github.com/ros-planning/navigation/issues/1069>`_) (`#1070 <https://github.com/ros-planning/navigation/issues/1070>`_)
|
||||
Co-authored-by: JF Dalbosco <jean-francois.dalbosco@safrangroup.com>
|
||||
* Fix future robot pose by Costmap2D (`#1066 <https://github.com/ros-planning/navigation/issues/1066>`_)
|
||||
Co-authored-by: Pavlo Kolomiiets <pavlo@blindnology.com>
|
||||
* use const-ref-getter for strings and vectors (`#1035 <https://github.com/ros-planning/navigation/issues/1035>`_)
|
||||
Co-authored-by: Dima Dorezyuk <dorezyuk@magazino.eu>
|
||||
* move enabled-logic to the LayeredCostmap (`#1036 <https://github.com/ros-planning/navigation/issues/1036>`_)
|
||||
Co-authored-by: Dima Dorezyuk <dorezyuk@magazino.eu>
|
||||
* cosmetic change (`#1055 <https://github.com/ros-planning/navigation/issues/1055>`_)
|
||||
* costmap_2d: Add missing package dependency to Eigen (`#1033 <https://github.com/ros-planning/navigation/issues/1033>`_)
|
||||
* Add invert_area_to_clear to clear costmap recovery behavior (`#1030 <https://github.com/ros-planning/navigation/issues/1030>`_)
|
||||
* Contributors: Atsushi Watanabe, ChristofDubs, Dhruv Maroo, Dima Dorezyuk, G.Doisy, Griswald Brooks, Noriaki Ando, Pavlo Kolomiiets, Yuki Furuta, Yukihiro Saito, easylyou, jdalbosc
|
||||
|
||||
1.17.1 (2020-08-27)
|
||||
-------------------
|
||||
* add explicit call to ros::Timer::stop in the destructor (`#984 <https://github.com/ros-planning/navigation/issues/984>`_)
|
||||
Co-authored-by: Dima Dorezyuk <dorezyuk@magazino.eu>
|
||||
* Exposing stopped\_ variable in costmap2D_ros with the getter function isStopped (`#913 <https://github.com/ros-planning/navigation/issues/913>`_)
|
||||
* use const for getter methods (`#948 <https://github.com/ros-planning/navigation/issues/948>`_)
|
||||
* Contributors: Dima Dorezyuk, Marco Bassa, Yuki Furuta
|
||||
|
||||
1.17.0 (2020-04-02)
|
||||
-------------------
|
||||
* Merge pull request `#982 <https://github.com/ros-planning/navigation/issues/982>`_ from ros-planning/noetic_prep
|
||||
Noetic Migration
|
||||
* fix invalid memory access
|
||||
* attempt to get test to run
|
||||
* increase required cmake version
|
||||
* Contributors: Michael Ferguson
|
||||
|
||||
1.16.6 (2020-03-18)
|
||||
-------------------
|
||||
|
||||
1.16.5 (2020-03-15)
|
||||
-------------------
|
||||
* [melodic] updated install for better portability. (`#973 <https://github.com/ros-planning/navigation/issues/973>`_)
|
||||
* Contributors: Sean Yen
|
||||
|
||||
1.16.4 (2020-03-04)
|
||||
-------------------
|
||||
* fix published footprint topic name (`#947 <https://github.com/cobalt-robotics/navigation/issues/947>`_)
|
||||
The default name of the topic will be updated in noetic
|
||||
* fix usage of size_locked, fixes `#959 <https://github.com/cobalt-robotics/navigation/issues/959>`_ (`#966 <https://github.com/cobalt-robotics/navigation/issues/966>`_)
|
||||
* [Windows][melodic] Navigation (except for map_server and amcl) Windows build bring up (`#851 <https://github.com/cobalt-robotics/navigation/issues/851>`_)
|
||||
* Merge pull request `#957 <https://github.com/cobalt-robotics/navigation/issues/957>`_ from Blindnology/melodic-updateOrigin
|
||||
Optimize costmap_2d::updateOrigin
|
||||
* Optimize costmap_2d::updateOrigin
|
||||
* Contributors: Michael Ferguson, Pavlo Kolomiiets, Sean Yen, Yuki Furuta
|
||||
|
||||
1.16.3 (2019-11-15)
|
||||
-------------------
|
||||
* Merge pull request `#877 <https://github.com/ros-planning/navigation/issues/877>`_ from SteveMacenski/layer_clear_area-melodic
|
||||
[melodic] moving clearing area method to costmap_layer so other applications can clear other types.
|
||||
* Merge branch 'melodic-devel' into layer_clear_area-melodic
|
||||
* Drop Parameter Magic (`#893 <https://github.com/ros-planning/navigation/issues/893>`_)
|
||||
* Fixes `#782 <https://github.com/ros-planning/navigation/issues/782>`_ (`#892 <https://github.com/ros-planning/navigation/issues/892>`_)
|
||||
* Costmap_2d plugin universal parameters and pre-hydro warnings (`#738 <https://github.com/ros-planning/navigation/issues/738>`_)
|
||||
* Comment and description clarification
|
||||
* Renamed resetOldParameters to loadOldParameters
|
||||
* Upscaled pre-hydro parameter info message to warning and added costmap-name
|
||||
* Warn user when static_map or map_type is set but not used while plugins are used
|
||||
* Added function that copies parent parameters inside each layer (makes it possible to set a global inflation_radius)
|
||||
* use parameter magic
|
||||
* Changed logic for when to resize layered costmap in static layer (`#792 <https://github.com/ros-planning/navigation/issues/792>`_)
|
||||
* Changed logic for when to resize layered costmap in static layer
|
||||
-Now the master layered costmap should no longer get resized when
|
||||
isSizeLocked returns true
|
||||
* Fixing format for if loop
|
||||
* clear area in layer for melodic
|
||||
* [kinetic] Fix Bounds Bug (plus test) (`#871 <https://github.com/ros-planning/navigation/issues/871>`_) (`#875 <https://github.com/ros-planning/navigation/issues/875>`_)
|
||||
* fix map bounds bug
|
||||
* Add test
|
||||
* Fix install (`#855 <https://github.com/ros-planning/navigation/issues/855>`_)
|
||||
* Add additional linked libraries (`#803 <https://github.com/ros-planning/navigation/issues/803>`_)
|
||||
* Contributors: David V. Lu!!, Martin Ganeff, Michael Ferguson, Steven Macenski, stevemacenski
|
||||
|
||||
1.16.2 (2018-07-31)
|
||||
-------------------
|
||||
* Merge pull request `#773 <https://github.com/ros-planning/navigation/issues/773>`_ from ros-planning/packaging_fixes
|
||||
packaging fixes
|
||||
* update costmap_2d cmake
|
||||
* explicit dependency on tf2
|
||||
* remove old PCL disable crap
|
||||
* Contributors: Michael Ferguson
|
||||
|
||||
1.16.1 (2018-07-28)
|
||||
-------------------
|
||||
* Merge pull request `#770 <https://github.com/ros-planning/navigation/issues/770>`_ from ros-planning/fix_debians
|
||||
Fix debian builds (closes `#769 <https://github.com/ros-planning/navigation/issues/769>`_)
|
||||
* add tf2_geometry_msgs depend to costmap_2d
|
||||
* Contributors: Michael Ferguson
|
||||
|
||||
1.16.0 (2018-07-25)
|
||||
-------------------
|
||||
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
|
||||
* unify combination_method dynamic reconfig, closes `#402 <https://github.com/ros-planning/navigation/issues/402>`_
|
||||
* Merge pull request `#723 <https://github.com/ros-planning/navigation/issues/723>`_ from moriarty/melodic-buildfarm-errors
|
||||
Melodic buildfarm errors
|
||||
* [costmap_2d/test] set empty transform to Identity
|
||||
* fix test: abs(unsigned int) is ambiguous
|
||||
Instead, compare values and subtract smaller from larger to find
|
||||
the dx and dy.
|
||||
* fixes pluginlib deprecated header warnings
|
||||
* Merge pull request `#694 <https://github.com/ros-planning/navigation/issues/694>`_ from ros-planning/lunar_691
|
||||
costmap variable init & cleanup (forward port of `#691 <https://github.com/ros-planning/navigation/issues/691>`_)
|
||||
* remove unused got_footprint\_
|
||||
* initialize all costmap variables
|
||||
* Merge pull request `#686 <https://github.com/ros-planning/navigation/issues/686>`_ from ros-planning/lunar_675
|
||||
Fixed race condition with costmaps in LayeredCostmap::resizeMap()
|
||||
* Fixed race condition with costmaps in LayeredCostmap::resizeMap()
|
||||
LayeredCostmap::updateMap() and LayeredCostmap::resizeMap() write to the master grid costmap.
|
||||
And these two functions can be called by different threads at the same time.
|
||||
One example of these cases is a race condition between subscriber callback thread
|
||||
dealing with dynamically-size-changing static_layer and periodical updateMap() calls from Costmap2DROS thread.
|
||||
Under the situation the master grid costmap is not thread-safe.
|
||||
LayeredCostmap::updateMap() already used the master grid costmap's lock.
|
||||
* Contributors: Alexander Moriarty, David V. Lu, Jaeyoung Lee, Michael Ferguson, Vincent Rabaud
|
||||
|
||||
1.15.2 (2018-03-22)
|
||||
-------------------
|
||||
* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
|
||||
update maintainer email (lunar)
|
||||
* Merge pull request `#670 <https://github.com/ros-planning/navigation/issues/670>`_ from DLu/fix206_lunar
|
||||
Fixes `#206 <https://github.com/ros-planning/navigation/issues/206>`_ for Lunar
|
||||
* fix 'enable' for static_layer with rolling window (`#659 <https://github.com/ros-planning/navigation/issues/659>`_) (`#665 <https://github.com/ros-planning/navigation/issues/665>`_)
|
||||
* Merge pull request `#649 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/564>`_)
|
||||
* Inflation Layer protected members and virtual computeCost [ABI BREAKING]
|
||||
* Fix for `#517 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/525>`_)
|
||||
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
|
||||
* add missing deps on libpcl
|
||||
* import only PCL common
|
||||
* pcl proagate -lQt5::Widgets flag so we need to find_package Qt5Widgets (`#578 <https://github.com/ros-planning/navigation/issues/578>`_)
|
||||
* Added deps to amcl costmap_2d move_base (`#512 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/205>`_
|
||||
* initialize publisher variables
|
||||
* Look for robot_radius when footprint is not set. `#206 <https://github.com/ros-planning/navigation/issues/206>`_
|
||||
* Add a first_map_only parameter so we keep reusing the first received static map
|
||||
* Merge pull request `#331 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/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 <https://github.com/ros-planning/navigation/issues/68>`_
|
||||
Fix for inflation memory problems
|
||||
InfIsValid `#128 <https://github.com/ros-planning/navigation/issues/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
|
||||
221
navigations/costmap_2d/CMakeLists.txt
Executable file
221
navigations/costmap_2d/CMakeLists.txt
Executable file
@@ -0,0 +1,221 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(costmap_2d)
|
||||
|
||||
find_package(catkin REQUIRED
|
||||
COMPONENTS
|
||||
cmake_modules
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
laser_geometry
|
||||
map_msgs
|
||||
message_filters
|
||||
message_generation
|
||||
nav_msgs
|
||||
pluginlib
|
||||
roscpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
tf2
|
||||
tf2_geometry_msgs
|
||||
tf2_ros
|
||||
tf2_sensor_msgs
|
||||
visualization_msgs
|
||||
voxel_grid
|
||||
)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(Boost REQUIRED COMPONENTS system thread)
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_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
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
LIBRARIES costmap_2d layers
|
||||
CATKIN_DEPENDS
|
||||
dynamic_reconfigure
|
||||
geometry_msgs
|
||||
laser_geometry
|
||||
map_msgs
|
||||
message_filters
|
||||
message_runtime
|
||||
nav_msgs
|
||||
pluginlib
|
||||
roscpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
tf2_ros
|
||||
visualization_msgs
|
||||
voxel_grid
|
||||
DEPENDS
|
||||
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
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(layers
|
||||
plugins/inflation_layer.cpp
|
||||
plugins/obstacle_layer.cpp
|
||||
plugins/static_layer.cpp
|
||||
plugins/voxel_layer.cpp
|
||||
plugins/preferred_layer.cpp
|
||||
plugins/unpreferred_layer.cpp
|
||||
plugins/critical_layer.cpp
|
||||
plugins/directional_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
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
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
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
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
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
## 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)
|
||||
|
||||
catkin_add_gtest(coordinates_test test/coordinates_test.cpp)
|
||||
target_link_libraries(coordinates_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}
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(FILES costmap_plugins.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
install(FILES test/TenByTen.yaml test/TenByTen.pgm
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test
|
||||
)
|
||||
|
||||
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
PATTERN ".svn" EXCLUDE
|
||||
)
|
||||
2
navigations/costmap_2d/README.md
Executable file
2
navigations/costmap_2d/README.md
Executable file
@@ -0,0 +1,2 @@
|
||||
# costmap_2d
|
||||
Thư viện xây dựng các lớp bản đồ chi phí
|
||||
23
navigations/costmap_2d/cfg/Costmap2D.cfg
Executable file
23
navigations/costmap_2d/cfg/Costmap2D.cfg
Executable file
@@ -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 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"))
|
||||
7
navigations/costmap_2d/cfg/GenericPlugin.cfg
Executable file
7
navigations/costmap_2d/cfg/GenericPlugin.cfg
Executable file
@@ -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"))
|
||||
12
navigations/costmap_2d/cfg/InflationPlugin.cfg
Executable file
12
navigations/costmap_2d/cfg/InflationPlugin.cfg
Executable file
@@ -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.", 15, 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"))
|
||||
20
navigations/costmap_2d/cfg/ObstaclePlugin.cfg
Executable file
20
navigations/costmap_2d/cfg/ObstaclePlugin.cfg
Executable file
@@ -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"))
|
||||
22
navigations/costmap_2d/cfg/VoxelPlugin.cfg
Executable file
22
navigations/costmap_2d/cfg/VoxelPlugin.cfg
Executable file
@@ -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"))
|
||||
37
navigations/costmap_2d/costmap_plugins.xml
Executable file
37
navigations/costmap_2d/costmap_plugins.xml
Executable file
@@ -0,0 +1,37 @@
|
||||
<class_libraries>
|
||||
<library path="liblayers">
|
||||
<class type="costmap_2d::InflationLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles.</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_2d::PreferredLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_2d::UnPreferredLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_2d::CriticalLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_2d::DirectionalLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
|
||||
</class>
|
||||
|
||||
|
||||
<class type="costmap_2d::ObstacleLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Listens to laser scan and point cloud messages and marks and clears grid cells.</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_2d::StaticLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_2d::VoxelLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
</class_libraries>
|
||||
51
navigations/costmap_2d/include/costmap_2d/array_parser.h
Executable file
51
navigations/costmap_2d/include/costmap_2d/array_parser.h
Executable file
@@ -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 <vector>
|
||||
#include <string>
|
||||
|
||||
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<std::vector<float> > parseVVF(const std::string& input, std::string& error_return);
|
||||
|
||||
} // end namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_ARRAY_PARSER_H
|
||||
50
navigations/costmap_2d/include/costmap_2d/cost_values.h
Executable file
50
navigations/costmap_2d/include/costmap_2d/cost_values.h
Executable file
@@ -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
|
||||
*********************************************************************/
|
||||
#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 CRITICAL_SPACE = 0;
|
||||
static const unsigned char FREE_SPACE = 60;
|
||||
static const unsigned char PREFERRED_SPACE = 20;
|
||||
static const unsigned char UNPREFERRED_SPACE = 100;
|
||||
}
|
||||
#endif // COSTMAP_2D_COST_VALUES_H_
|
||||
469
navigations/costmap_2d/include/costmap_2d/costmap_2d.h
Executable file
469
navigations/costmap_2d/include/costmap_2d/costmap_2d.h
Executable file
@@ -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 <vector>
|
||||
#include <queue>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
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 <b>are not guaranteed to lie within the map.</b>
|
||||
*/
|
||||
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<geometry_msgs::Point>& 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<MapLocation>& polygon, std::vector<MapLocation>& 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<MapLocation>& polygon, std::vector<MapLocation>& 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<typename data_type>
|
||||
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<class ActionType>
|
||||
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<class ActionType>
|
||||
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<MapLocation>& 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<MapLocation>& cells_;
|
||||
};
|
||||
};
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_COSTMAP_2D_H
|
||||
108
navigations/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
Executable file
108
navigations/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
Executable file
@@ -0,0 +1,108 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <map_msgs/OccupancyGridUpdate.h>
|
||||
|
||||
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
|
||||
282
navigations/costmap_2d/include/costmap_2d/costmap_2d_ros.h
Executable file
282
navigations/costmap_2d/include/costmap_2d/costmap_2d_ros.h
Executable file
@@ -0,0 +1,282 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/costmap_2d_publisher.h>
|
||||
#include <costmap_2d/Costmap2DConfig.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <geometry_msgs/Polygon.h>
|
||||
#include <geometry_msgs/PolygonStamped.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
|
||||
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<XmlRpc::XmlRpcValue>(*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(const std::string &name, tf2_ros::Buffer& 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() const
|
||||
{
|
||||
return layered_costmap_->isCurrent();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Is the costmap stopped
|
||||
*/
|
||||
bool isStopped() const
|
||||
{
|
||||
return stopped_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @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(geometry_msgs::PoseStamped& global_pose) const;
|
||||
|
||||
/** @brief Returns costmap name */
|
||||
inline const std::string& getName() const noexcept
|
||||
{
|
||||
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() const
|
||||
{
|
||||
return layered_costmap_->getCostmap();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the global frame of the costmap
|
||||
* @return The global frame of the costmap
|
||||
*/
|
||||
inline const std::string& getGlobalFrameID() const noexcept
|
||||
{
|
||||
return global_frame_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the local frame of the costmap
|
||||
* @return The local frame of the costmap
|
||||
*/
|
||||
inline const std::string& getBaseFrameID() const noexcept
|
||||
{
|
||||
return robot_base_frame_;
|
||||
}
|
||||
LayeredCostmap* getLayeredCostmap() const
|
||||
{
|
||||
return layered_costmap_;
|
||||
}
|
||||
|
||||
/** @brief Returns the current padded footprint as a geometry_msgs::Polygon. */
|
||||
geometry_msgs::Polygon getRobotFootprintPolygon() const
|
||||
{
|
||||
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. */
|
||||
inline const std::vector<geometry_msgs::Point>& getRobotFootprint() const noexcept
|
||||
{
|
||||
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. */
|
||||
inline const std::vector<geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
|
||||
{
|
||||
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<geometry_msgs::Point>& 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<geometry_msgs::Point>& 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_;
|
||||
tf2_ros::Buffer& 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 loadOldParameters(ros::NodeHandle& nh);
|
||||
void warnForOldParameters(ros::NodeHandle& nh);
|
||||
void checkOldParam(ros::NodeHandle& nh, const std::string ¶m_name);
|
||||
void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, 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_;
|
||||
boost::thread* map_update_thread_; ///< @brief A thread for updating the map
|
||||
ros::Time last_publish_;
|
||||
ros::Duration publish_cycle;
|
||||
pluginlib::ClassLoader<Layer> plugin_loader_;
|
||||
Costmap2DPublisher* publisher_;
|
||||
dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
|
||||
|
||||
boost::recursive_mutex configuration_mutex_;
|
||||
|
||||
ros::Subscriber footprint_sub_;
|
||||
ros::Publisher footprint_pub_;
|
||||
std::vector<geometry_msgs::Point> unpadded_footprint_;
|
||||
std::vector<geometry_msgs::Point> padded_footprint_;
|
||||
float footprint_padding_;
|
||||
costmap_2d::Costmap2DConfig old_config_;
|
||||
};
|
||||
// class Costmap2DROS
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_COSTMAP_2D_ROS_H
|
||||
151
navigations/costmap_2d/include/costmap_2d/costmap_layer.h
Executable file
151
navigations/costmap_2d/include/costmap_2d/costmap_layer.h
Executable file
@@ -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_COSTMAP_LAYER_H_
|
||||
#define COSTMAP_2D_COSTMAP_LAYER_H_
|
||||
#include <ros/ros.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
|
||||
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();
|
||||
|
||||
virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area=false);
|
||||
|
||||
/**
|
||||
* 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_
|
||||
69
navigations/costmap_2d/include/costmap_2d/costmap_math.h
Executable file
69
navigations/costmap_2d/include/costmap_2d/costmap_math.h
Executable file
@@ -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 <math.h>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
/** @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<geometry_msgs::Point>& polygon, float testx, float testy);
|
||||
|
||||
bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
|
||||
|
||||
#endif // COSTMAP_2D_COSTMAP_MATH_H_
|
||||
19
navigations/costmap_2d/include/costmap_2d/critical_layer.h
Executable file
19
navigations/costmap_2d/include/costmap_2d/critical_layer.h
Executable file
@@ -0,0 +1,19 @@
|
||||
#ifndef COSTMAP_2D_CRITICAL_LAYER_H_
|
||||
#define COSTMAP_2D_CRITICAL_LAYER_H_
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
class CriticalLayer : public StaticLayer
|
||||
{
|
||||
public:
|
||||
CriticalLayer();
|
||||
virtual ~CriticalLayer();
|
||||
private:
|
||||
unsigned char interpretValue(unsigned char value);
|
||||
void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
|
||||
};
|
||||
}
|
||||
|
||||
#endif // COSTMAP_2D_CRITICAL_LAYER_H_
|
||||
37
navigations/costmap_2d/include/costmap_2d/directional_layer.h
Executable file
37
navigations/costmap_2d/include/costmap_2d/directional_layer.h
Executable file
@@ -0,0 +1,37 @@
|
||||
#ifndef COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
|
||||
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
class DirectionalLayer : public StaticLayer
|
||||
{
|
||||
public:
|
||||
DirectionalLayer();
|
||||
virtual ~DirectionalLayer();
|
||||
bool laneFilter(const std::vector<geometry_msgs::PoseStamped> plan);
|
||||
void resetMap();
|
||||
|
||||
private:
|
||||
void incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map);
|
||||
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
|
||||
unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot);
|
||||
// void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
|
||||
void laneFilter(std::vector<std::pair<unsigned int, double>> x, std::vector<std::pair<unsigned int, double>> y, std::vector<double> yaw_robot);
|
||||
bool inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance);
|
||||
void convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
|
||||
bool getRobotPose(double &x, double &y, double &yaw);
|
||||
|
||||
std::vector<std::array<uint16_t, 2>> directional_areas_;
|
||||
double pose_x_, pose_y_, pose_yaw_;
|
||||
double distance_skip_ = 1.0;
|
||||
ros::Publisher lane_mask_pub_;
|
||||
nav_msgs::OccupancyGrid new_map_;
|
||||
tf::TransformListener listener_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif // COSTMAP_2D_DIRECTIONAL_LAYER_H_
|
||||
147
navigations/costmap_2d/include/costmap_2d/footprint.h
Executable file
147
navigations/costmap_2d/include/costmap_2d/footprint.h
Executable file
@@ -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 <ros/ros.h>
|
||||
#include <geometry_msgs/Polygon.h>
|
||||
#include <geometry_msgs/PolygonStamped.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <geometry_msgs/Point32.h>
|
||||
|
||||
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<geometry_msgs::Point>& 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<geometry_msgs::Point> pts);
|
||||
|
||||
/**
|
||||
* @brief Convert Polygon msg to vector of Points.
|
||||
*/
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point>& footprint_spec,
|
||||
std::vector<geometry_msgs::Point>& 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<geometry_msgs::Point>& footprint_spec,
|
||||
geometry_msgs::PolygonStamped & oriented_footprint);
|
||||
|
||||
/**
|
||||
* @brief Adds the specified amount of padding to the footprint (in place)
|
||||
*/
|
||||
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding);
|
||||
|
||||
/**
|
||||
* @brief Create a circular footprint from a given radius
|
||||
*/
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point>& footprint);
|
||||
|
||||
/**
|
||||
* @brief Read the ros-params "footprint" and/or "robot_radius" from
|
||||
* the given NodeHandle using searchParam() to go up the tree.
|
||||
*/
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point> 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<geometry_msgs::Point>& footprint);
|
||||
|
||||
} // end namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_FOOTPRINT_H
|
||||
200
navigations/costmap_2d/include/costmap_2d/inflation_layer.h
Executable file
200
navigations/costmap_2d/include/costmap_2d/inflation_layer.h
Executable file
@@ -0,0 +1,200 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/InflationPluginConfig.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
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_;
|
||||
if (seen_)
|
||||
delete[] seen_;
|
||||
}
|
||||
|
||||
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<double, std::vector<CellData> > 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<costmap_2d::InflationPluginConfig> *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_
|
||||
151
navigations/costmap_2d/include/costmap_2d/layer.h
Executable file
151
navigations/costmap_2d/include/costmap_2d/layer.h
Executable file
@@ -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: David V. Lu!!
|
||||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_LAYER_H_
|
||||
#define COSTMAP_2D_LAYER_H_
|
||||
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <string>
|
||||
#include <tf2_ros/buffer.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
class LayeredCostmap;
|
||||
|
||||
class Layer
|
||||
{
|
||||
public:
|
||||
Layer();
|
||||
|
||||
void initialize(LayeredCostmap* parent, std::string name, tf2_ros::Buffer *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 getter if the current layer is enabled.
|
||||
*
|
||||
* The user may enable/disable a layer though dynamic reconfigure.
|
||||
* Disabled layers won't receive calls to
|
||||
* - Layer::updateCosts
|
||||
* - Layer::updateBounds
|
||||
* - Layer::isCurrent
|
||||
* from the LayeredCostmap.
|
||||
*
|
||||
* Calls to Layer::activate, Layer::deactivate and Layer::reset won't be
|
||||
* blocked.
|
||||
*/
|
||||
inline bool isEnabled() const noexcept
|
||||
{
|
||||
return enabled_;
|
||||
}
|
||||
|
||||
/** @brief Implement this to make this layer match the size of the parent costmap. */
|
||||
virtual void matchSize() {}
|
||||
|
||||
inline const std::string& getName() const noexcept
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
|
||||
/** @brief Convenience function for layered_costmap_->getFootprint(). */
|
||||
const std::vector<geometry_msgs::Point>& 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_;
|
||||
std::string name_;
|
||||
tf2_ros::Buffer *tf_;
|
||||
|
||||
private:
|
||||
std::vector<geometry_msgs::Point> footprint_spec_;
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_LAYER_H_
|
||||
177
navigations/costmap_2d/include/costmap_2d/layered_costmap.h
Executable file
177
navigations/costmap_2d/include/costmap_2d/layered_costmap.h
Executable file
@@ -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 <costmap_2d/cost_values.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
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);
|
||||
|
||||
inline const std::string& getGlobalFrameID() const noexcept
|
||||
{
|
||||
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<boost::shared_ptr<Layer> >* getPlugins()
|
||||
{
|
||||
return &plugins_;
|
||||
}
|
||||
|
||||
void addPlugin(boost::shared_ptr<Layer> 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<geometry_msgs::Point>& footprint_spec);
|
||||
|
||||
/** @brief Returns the latest footprint stored with setFootprint(). */
|
||||
const std::vector<geometry_msgs::Point>& 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<boost::shared_ptr<Layer> > plugins_;
|
||||
|
||||
bool initialized_;
|
||||
bool size_locked_;
|
||||
double circumscribed_radius_, inscribed_radius_;
|
||||
std::vector<geometry_msgs::Point> footprint_;
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_LAYERED_COSTMAP_H_
|
||||
102
navigations/costmap_2d/include/costmap_2d/observation.h
Executable file
102
navigations/costmap_2d/include/costmap_2d/observation.h
Executable file
@@ -0,0 +1,102 @@
|
||||
/*
|
||||
* 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 <geometry_msgs/Point.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
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 sensor_msgs::PointCloud2()), 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, const sensor_msgs::PointCloud2 &cloud,
|
||||
double obstacle_range, double raytrace_range) :
|
||||
origin_(origin), cloud_(new sensor_msgs::PointCloud2(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 sensor_msgs::PointCloud2(*(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(const sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
|
||||
cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
|
||||
{
|
||||
}
|
||||
|
||||
geometry_msgs::Point origin_;
|
||||
sensor_msgs::PointCloud2* cloud_;
|
||||
double obstacle_range_, raytrace_range_;
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
#endif // COSTMAP_2D_OBSERVATION_H_
|
||||
154
navigations/costmap_2d/include/costmap_2d/observation_buffer.h
Executable file
154
navigations/costmap_2d/include/costmap_2d/observation_buffer.h
Executable file
@@ -0,0 +1,154 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <vector>
|
||||
#include <list>
|
||||
#include <string>
|
||||
#include <ros/time.h>
|
||||
#include <costmap_2d/observation.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
// Thread support
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
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 tf2_buffer A reference to a tf2 Buffer
|
||||
* @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, tf2_ros::Buffer& tf2_buffer, 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
|
||||
* <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
|
||||
* @param cloud The cloud to be buffered
|
||||
*/
|
||||
void bufferCloud(const sensor_msgs::PointCloud2& 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<Observation>& 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();
|
||||
|
||||
tf2_ros::Buffer& tf2_buffer_;
|
||||
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> 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_
|
||||
177
navigations/costmap_2d/include/costmap_2d/obstacle_layer.h
Executable file
177
navigations/costmap_2d/include/costmap_2d/obstacle_layer.h
Executable file
@@ -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 <ros/ros.h>
|
||||
#include <costmap_2d/costmap_layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <laser_geometry/laser_geometry.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
#include <tf2_ros/message_filter.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <costmap_2d/ObstaclePluginConfig.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
|
||||
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<costmap_2d::ObservationBuffer>& 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<ObservationBuffer>& 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<costmap_2d::ObservationBuffer>& 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<costmap_2d::ObservationBuffer>& 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<costmap_2d::Observation>& 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<costmap_2d::Observation>& 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<geometry_msgs::Point> 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<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_; ///< @brief Used for the observation message filters
|
||||
std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
|
||||
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
|
||||
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
|
||||
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
|
||||
|
||||
// Used only for testing purposes
|
||||
std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
|
||||
|
||||
bool rolling_window_;
|
||||
dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig> *dsrv_;
|
||||
|
||||
int combination_method_;
|
||||
|
||||
private:
|
||||
void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level);
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_OBSTACLE_LAYER_H_
|
||||
18
navigations/costmap_2d/include/costmap_2d/preferred_layer.h
Executable file
18
navigations/costmap_2d/include/costmap_2d/preferred_layer.h
Executable file
@@ -0,0 +1,18 @@
|
||||
#ifndef COSTMAP_2D_PREFERRED_LAYER_H_
|
||||
#define COSTMAP_2D_PREFERRED_LAYER_H_
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
class PreferredLayer : public StaticLayer
|
||||
{
|
||||
public:
|
||||
PreferredLayer();
|
||||
virtual ~PreferredLayer();
|
||||
private:
|
||||
unsigned char interpretValue(unsigned char value);
|
||||
};
|
||||
}
|
||||
|
||||
#endif // COSTMAP_2D_PREFERRED_LAYER_
|
||||
102
navigations/costmap_2d/include/costmap_2d/static_layer.h
Executable file
102
navigations/costmap_2d/include/costmap_2d/static_layer.h
Executable file
@@ -0,0 +1,102 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <costmap_2d/costmap_layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/GenericPluginConfig.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <map_msgs/OccupancyGridUpdate.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <string>
|
||||
|
||||
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();
|
||||
protected:
|
||||
virtual unsigned char interpretValue(unsigned char value);
|
||||
virtual void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);
|
||||
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update);
|
||||
unsigned char* threshold_;
|
||||
std::string base_frame_id_;
|
||||
std::string global_frame_; ///< @brief The global frame for the costmap
|
||||
std::string map_frame_; /// @brief frame that map is located in
|
||||
std::string map_topic_; // Hiep thêm vào mục đich phân biết zones
|
||||
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_;
|
||||
|
||||
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 reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
|
||||
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
#endif // COSTMAP_2D_STATIC_LAYER_H_
|
||||
99
navigations/costmap_2d/include/costmap_2d/testing_helper.h
Executable file
99
navigations/costmap_2d/include/costmap_2d/testing_helper.h
Executable file
@@ -0,0 +1,99 @@
|
||||
#ifndef COSTMAP_2D_TESTING_HELPER_H
|
||||
#define COSTMAP_2D_TESTING_HELPER_H
|
||||
|
||||
#include<costmap_2d/cost_values.h>
|
||||
#include<costmap_2d/costmap_2d.h>
|
||||
#include <costmap_2d/static_layer.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/inflation_layer.h>
|
||||
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
const double MAX_Z(1.0);
|
||||
|
||||
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, tf2_ros::Buffer& tf)
|
||||
{
|
||||
costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
|
||||
layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
|
||||
slayer->initialize(&layers, "static", &tf);
|
||||
}
|
||||
|
||||
costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
|
||||
{
|
||||
costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
|
||||
olayer->initialize(&layers, "obstacles", &tf);
|
||||
layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(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){
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
sensor_msgs::PointCloud2Modifier modifier(cloud);
|
||||
modifier.setPointCloud2FieldsByString(1, "xyz");
|
||||
modifier.resize(1);
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
|
||||
*iter_x = x;
|
||||
*iter_y = y;
|
||||
*iter_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, tf2_ros::Buffer& tf)
|
||||
{
|
||||
costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
|
||||
ilayer->initialize(&layers, "inflation", &tf);
|
||||
boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
|
||||
layers.addPlugin(ipointer);
|
||||
return ilayer;
|
||||
}
|
||||
|
||||
|
||||
#endif // COSTMAP_2D_TESTING_HELPER_H
|
||||
21
navigations/costmap_2d/include/costmap_2d/unpreferred_layer.h
Executable file
21
navigations/costmap_2d/include/costmap_2d/unpreferred_layer.h
Executable file
@@ -0,0 +1,21 @@
|
||||
#ifndef COSTMAP_2D_UNPREFERRED_LAYER_H_
|
||||
#define COSTMAP_2D_UNPREFERRED_LAYER_H_
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
class UnPreferredLayer : public StaticLayer
|
||||
{
|
||||
public:
|
||||
UnPreferredLayer();
|
||||
virtual ~UnPreferredLayer();
|
||||
|
||||
private:
|
||||
unsigned char interpretValue(unsigned char value);
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // COSTMAP_2D_UNPREFERRED_LAYER_
|
||||
150
navigations/costmap_2d/include/costmap_2d/voxel_layer.h
Executable file
150
navigations/costmap_2d/include/costmap_2d/voxel_layer.h
Executable file
@@ -0,0 +1,150 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/VoxelGrid.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <laser_geometry/laser_geometry.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <costmap_2d/VoxelPluginConfig.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
|
||||
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<costmap_2d::VoxelPluginConfig> *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_
|
||||
21
navigations/costmap_2d/launch/example.launch
Executable file
21
navigations/costmap_2d/launch/example.launch
Executable file
@@ -0,0 +1,21 @@
|
||||
<launch>
|
||||
<!--
|
||||
NOTE: You'll need to bring up something that publishes sensor data (see
|
||||
rosstage), something that publishes a map (see map_server), and something to
|
||||
visualize a costmap (see nav_view), to see things work.
|
||||
|
||||
Also, on a real robot, you'd want to set the "use_sim_time" parameter to false, or just not set it.
|
||||
-->
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
|
||||
<!-- Publishes the voxel grid to rviz for display -->
|
||||
<node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
|
||||
<remap from="voxel_grid" to="costmap/voxel_grid"/>
|
||||
</node>
|
||||
|
||||
<!-- Run the costmap node -->
|
||||
<node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" >
|
||||
<rosparam file="$(find costmap_2d)/launch/example_params.yaml" command="load" ns="costmap" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
40
navigations/costmap_2d/launch/example_params.yaml
Executable file
40
navigations/costmap_2d/launch/example_params.yaml
Executable file
@@ -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}
|
||||
8
navigations/costmap_2d/msg/VoxelGrid.msg
Executable file
8
navigations/costmap_2d/msg/VoxelGrid.msg
Executable file
@@ -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
|
||||
|
||||
59
navigations/costmap_2d/package.xml
Executable file
59
navigations/costmap_2d/package.xml
Executable file
@@ -0,0 +1,59 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="2">
|
||||
<name>costmap_2d</name>
|
||||
<version>1.17.3</version>
|
||||
<description>
|
||||
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.
|
||||
</description>
|
||||
<author>Eitan Marder-Eppstein</author>
|
||||
<author>David V. Lu!!</author>
|
||||
<author>Dave Hershberger</author>
|
||||
<author>contradict@gmail.com</author>
|
||||
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
|
||||
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
|
||||
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
|
||||
<license>BSD</license>
|
||||
<url>http://wiki.ros.org/costmap_2d</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>cmake_modules</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>tf2_geometry_msgs</build_depend>
|
||||
<build_depend>tf2_sensor_msgs</build_depend>
|
||||
|
||||
<depend>dynamic_reconfigure</depend>
|
||||
<depend>eigen</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>laser_geometry</depend>
|
||||
<depend>map_msgs</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>rostest</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>voxel_grid</depend>
|
||||
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<exec_depend>rosconsole</exec_depend>
|
||||
<test_depend>map_server</test_depend>
|
||||
<test_depend>rosbag</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>rosunit</test_depend>
|
||||
|
||||
<export>
|
||||
<costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
37
navigations/costmap_2d/plugins/critical_layer.cpp
Executable file
37
navigations/costmap_2d/plugins/critical_layer.cpp
Executable file
@@ -0,0 +1,37 @@
|
||||
#include <costmap_2d/critical_layer.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_2d::CriticalLayer, costmap_2d::Layer)
|
||||
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::CRITICAL_SPACE;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
CriticalLayer::CriticalLayer(){}
|
||||
CriticalLayer::~CriticalLayer(){}
|
||||
|
||||
unsigned char CriticalLayer::interpretValue(unsigned char value)
|
||||
{
|
||||
// check if the static value is above the unknown or lethal thresholds
|
||||
if(value >= *this->threshold_)
|
||||
return CRITICAL_SPACE;
|
||||
else
|
||||
return NO_INFORMATION;
|
||||
}
|
||||
|
||||
void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
||||
{
|
||||
if (!map_received_)
|
||||
return;
|
||||
|
||||
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
|
||||
if (!use_maximum_)
|
||||
updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
|
||||
else
|
||||
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
|
||||
|
||||
}
|
||||
}
|
||||
412
navigations/costmap_2d/plugins/directional_layer.cpp
Executable file
412
navigations/costmap_2d/plugins/directional_layer.cpp
Executable file
@@ -0,0 +1,412 @@
|
||||
#include <costmap_2d/directional_layer.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf/transform_datatypes.h>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_2d::DirectionalLayer, costmap_2d::Layer)
|
||||
|
||||
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
|
||||
|
||||
using costmap_2d::CRITICAL_SPACE;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
DirectionalLayer::DirectionalLayer()
|
||||
{
|
||||
ros::NodeHandle nh("~/" + name_);
|
||||
lane_mask_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/direction_zones/lanes", 1);
|
||||
}
|
||||
DirectionalLayer::~DirectionalLayer() {}
|
||||
|
||||
bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)
|
||||
{
|
||||
if (directional_areas_.empty())
|
||||
throw "Has no map data";
|
||||
nav_msgs::Path path;
|
||||
path.header.stamp = ros::Time::now();
|
||||
path.header.frame_id = map_frame_;
|
||||
path.poses = plan;
|
||||
return this->laneFilter(directional_areas_, path);
|
||||
}
|
||||
|
||||
void DirectionalLayer::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 / 2, size_y / 2, 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 / 2 ||
|
||||
master->getSizeInCellsY() != size_y / 2 ||
|
||||
master->getResolution() != new_map->info.resolution ||
|
||||
master->getOriginX() != new_map->info.origin.position.x ||
|
||||
master->getOriginY() != new_map->info.origin.position.y))
|
||||
{
|
||||
// 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 / 2, size_y / 2, new_map->info.resolution);
|
||||
layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map->info.resolution, new_map->info.origin.position.x,
|
||||
new_map->info.origin.position.y,
|
||||
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
|
||||
}
|
||||
else if (size_x_ != size_x / 2 || size_y_ != size_y / 2 ||
|
||||
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 / 2, size_y / 2, new_map->info.resolution);
|
||||
resizeMap(size_x / 2, size_y / 2, new_map->info.resolution,
|
||||
new_map->info.origin.position.x, new_map->info.origin.position.y);
|
||||
}
|
||||
unsigned char values[4];
|
||||
directional_areas_.clear();
|
||||
directional_areas_.resize(size_x / 2 * size_y / 2);
|
||||
// initialize the costmap with static data
|
||||
for (unsigned int iy = 0; iy < size_y; iy += 2)
|
||||
{
|
||||
for (unsigned int ix = 0; ix < size_x; ix += 2)
|
||||
{
|
||||
values[0] = new_map->data[MAP_IDX(size_x, ix, iy)];
|
||||
values[1] = new_map->data[MAP_IDX(size_x, ix + 1, iy)];
|
||||
values[2] = new_map->data[MAP_IDX(size_x, ix, iy + 1)];
|
||||
values[3] = new_map->data[MAP_IDX(size_x, ix + 1, iy + 1)];
|
||||
uint32_t color_avg = (uint32_t)(values[0] | values[1] << 8u | values[2] << 16u | values[3] << 24u);
|
||||
int index = getIndex(ix / 2, iy / 2);
|
||||
directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
|
||||
directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
|
||||
costmap_[index] = costmap_2d::NO_INFORMATION;
|
||||
}
|
||||
}
|
||||
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;
|
||||
|
||||
new_map_.header = new_map->header;
|
||||
new_map_.header.stamp = ros::Time::now();
|
||||
new_map_.info = new_map->info;
|
||||
new_map_.info.width = width_;
|
||||
new_map_.info.height = height_;
|
||||
|
||||
// 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();
|
||||
}
|
||||
}
|
||||
|
||||
bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
|
||||
if (new_map.empty())
|
||||
return false;
|
||||
|
||||
std::vector<std::pair<unsigned int, double>> X_List, Y_List;
|
||||
std::vector<double> Yaw_list;
|
||||
|
||||
const geometry_msgs::PoseStamped &e = path.poses.back();
|
||||
|
||||
bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
|
||||
if(!get_success) return false;
|
||||
for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
|
||||
{
|
||||
const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
|
||||
unsigned int mx, my;
|
||||
if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
|
||||
{
|
||||
ROS_ERROR("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
|
||||
return false;
|
||||
}
|
||||
// Convert to yaw
|
||||
tf::Quaternion quaternion;
|
||||
tf::quaternionMsgToTF(p.pose.orientation, quaternion);
|
||||
double theta = tf::getYaw(quaternion);
|
||||
unsigned char value = laneFilter(mx, my, theta);
|
||||
if (get_success)
|
||||
{
|
||||
if (
|
||||
(!inSkipErea(pose_x_, pose_y_, p.pose.position.x, p.pose.position.y, distance_skip_) &
|
||||
!inSkipErea(p.pose.position.x, p.pose.position.y, e.pose.position.x, e.pose.position.y, distance_skip_))
|
||||
|| p == path.poses.back()
|
||||
)
|
||||
{
|
||||
if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE)
|
||||
{
|
||||
std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
|
||||
std::pair<unsigned int, double> y_val(my, p.pose.position.y);
|
||||
X_List.push_back(x_val);
|
||||
Y_List.push_back(y_val);
|
||||
Yaw_list.push_back(theta);
|
||||
// costmap_[getIndex(mx, my)] = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!Yaw_list.empty())
|
||||
{
|
||||
laneFilter(X_List, Y_List, Yaw_list);
|
||||
nav_msgs::OccupancyGrid lanes;
|
||||
convertToMap(costmap_, lanes, 0.65, 0.196);
|
||||
lane_mask_pub_.publish(lanes);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
|
||||
{
|
||||
dir_map.header = new_map_.header;
|
||||
dir_map.header.stamp = ros::Time::now();
|
||||
dir_map.info = new_map_.info;
|
||||
int size_costmap = new_map_.info.width * new_map_.info.height;
|
||||
dir_map.data.resize(size_costmap);
|
||||
for (int i = 0; i < size_costmap; i++)
|
||||
{
|
||||
int8_t value;
|
||||
if (costmap[i] == costmap_2d::NO_INFORMATION)
|
||||
{
|
||||
value = -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
double occ = (costmap[i]) / 255.0;
|
||||
if (occ > occ_th)
|
||||
value = +100;
|
||||
else if (occ < free_th)
|
||||
value = 0;
|
||||
else
|
||||
{
|
||||
double ratio = (occ - free_th) / (occ_th - free_th);
|
||||
value = 1 + 98 * ratio;
|
||||
}
|
||||
}
|
||||
dir_map.data[i] = value;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
|
||||
{
|
||||
double yaw_lane;
|
||||
unsigned char cost = costmap_2d::NO_INFORMATION;
|
||||
|
||||
int index = getIndex(x, y);
|
||||
for (auto &lane : directional_areas_[index])
|
||||
{
|
||||
if (lane > 359)
|
||||
{
|
||||
cost = std::min(cost, costmap_2d::NO_INFORMATION);
|
||||
}
|
||||
else
|
||||
{
|
||||
double yaw_lane = (double)lane / 180 * M_PI;
|
||||
if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
|
||||
cost = std::min(cost, costmap_2d::FREE_SPACE);
|
||||
else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
|
||||
cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
|
||||
else
|
||||
cost = std::min(cost, costmap_2d::NO_INFORMATION);
|
||||
}
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
void DirectionalLayer::laneFilter(std::vector<std::pair<unsigned int, double>> x,
|
||||
std::vector<std::pair<unsigned int, double>> y,
|
||||
std::vector<double> yaw_robot)
|
||||
{
|
||||
if(x.empty() || y.empty() || yaw_robot.empty())
|
||||
return;
|
||||
|
||||
unsigned int x_min, y_min, x_max, y_max;
|
||||
double x_min_w, y_min_w, x_max_w, y_max_w;
|
||||
|
||||
x_min = x.front().first; y_min = y.front().first;
|
||||
x_max = x.front().first; y_max = y.front().first;
|
||||
x_min_w = x.front().second; y_min_w = y.front().second;
|
||||
x_max_w = x.front().second; y_max_w = y.front().second;
|
||||
for (int i = 1; i < yaw_robot.size(); i++)
|
||||
{
|
||||
if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
|
||||
continue;
|
||||
x_min = std::min(x_min, x[i].first);
|
||||
y_min = std::min(y_min, y[i].first);
|
||||
x_max = std::max(x_max, x[i].first);
|
||||
y_max = std::max(y_max, y[i].first);
|
||||
|
||||
x_min_w = std::min(x_min_w, x[i].second);
|
||||
y_min_w = std::min(y_min_w, y[i].second);
|
||||
x_max_w = std::max(x_max_w, x[i].second);
|
||||
y_max_w = std::max(y_max_w, y[i].second);
|
||||
|
||||
}
|
||||
ROS_INFO("%d %d %d %d", x_min, y_min, x_max, y_max);
|
||||
ROS_INFO("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w);
|
||||
for (int i = 0; i < yaw_robot.size(); i++)
|
||||
{
|
||||
if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
|
||||
continue;
|
||||
|
||||
double yaw_rad = (double)yaw_robot[i] / 180 * M_PI;
|
||||
if (fabs(cos(yaw_rad)) > fabs(sin(yaw_rad)))
|
||||
{
|
||||
int x_ = x[i].first;
|
||||
// Dưới lên trên
|
||||
for (int j = y[i].first; j <= y_max; j++)
|
||||
{
|
||||
int y_ = j;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
// Trên xuống dưới
|
||||
for (int k = y[i].first; k >= y_min; k--)
|
||||
{
|
||||
int y_ = k;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
|
||||
int y_ = y[i].first;
|
||||
// Phải qua trái
|
||||
for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
|
||||
{
|
||||
int x_ = j;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
// Trái qua phải
|
||||
for (int k = x[i].first; k >= 0; k--)
|
||||
{
|
||||
int x_ = k;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
}
|
||||
else if(fabs(cos(yaw_rad)) < fabs(sin(yaw_rad)))
|
||||
{
|
||||
int y_ = y[i].first;
|
||||
// Phải qua trái
|
||||
for (int j = x[i].first; j <= x_max; j++)
|
||||
{
|
||||
int x_ = j;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
// Trái qua phải
|
||||
for (int k = x[i].first; k >= x_min; k--)
|
||||
{
|
||||
int x_ = k;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
|
||||
int x_ = x[i].first;
|
||||
// Dưới lên trên
|
||||
for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
|
||||
{
|
||||
int y_ = j;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
// Trên xuống dưới
|
||||
for (int k = y[i].first; k >= 0; k--)
|
||||
{
|
||||
int y_ = k;
|
||||
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
|
||||
continue;
|
||||
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
|
||||
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
|
||||
costmap_[getIndex(x_, y_)] = value;
|
||||
else
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DirectionalLayer::resetMap()
|
||||
{
|
||||
unsigned int size_x = layered_costmap_->getCostmap()->getSizeInCellsX();
|
||||
unsigned int size_y = layered_costmap_->getCostmap()->getSizeInCellsY();
|
||||
for (unsigned int iy = 0; iy < size_y; iy++)
|
||||
{
|
||||
for (unsigned int ix = 0; ix < size_x; ix++)
|
||||
{
|
||||
int index = getIndex(ix, iy);
|
||||
costmap_[index] = costmap_2d::NO_INFORMATION;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw)
|
||||
{
|
||||
tf::StampedTransform transform;
|
||||
try
|
||||
{
|
||||
listener_.lookupTransform(map_frame_, base_frame_id_, ros::Time(0), transform);
|
||||
}
|
||||
catch (tf::TransformException &ex)
|
||||
{
|
||||
ROS_ERROR("%s", ex.what());
|
||||
return false;
|
||||
}
|
||||
x = transform.getOrigin().x();
|
||||
y = transform.getOrigin().y();
|
||||
// Extract the rotation as a quaternion from the transform
|
||||
tf::Quaternion rotation = transform.getRotation();
|
||||
// Convert the quaternion to a yaw angle (in radians)
|
||||
yaw = tf::getYaw(rotation);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DirectionalLayer::inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance)
|
||||
{
|
||||
return fabs(hypot(start_x - end_x, start_y - end_y)) <= skip_distance;
|
||||
}
|
||||
}
|
||||
385
navigations/costmap_2d/plugins/inflation_layer.cpp
Executable file
385
navigations/costmap_2d/plugins/inflation_layer.cpp
Executable file
@@ -0,0 +1,385 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <algorithm>
|
||||
#include <costmap_2d/inflation_layer.h>
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
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()
|
||||
: resolution_(0)
|
||||
, inflation_radius_(0)
|
||||
, inscribed_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<float>::max())
|
||||
, last_min_y_(-std::numeric_limits<float>::max())
|
||||
, last_max_x_(std::numeric_limits<float>::max())
|
||||
, last_max_y_(std::numeric_limits<float>::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<costmap_2d::InflationPluginConfig>::CallbackType cb =
|
||||
[this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
|
||||
if (dsrv_ != NULL){
|
||||
dsrv_->clearCallback();
|
||||
dsrv_->setCallback(cb);
|
||||
}
|
||||
else
|
||||
{
|
||||
dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(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 -<double>::max() it does not
|
||||
// work with Costmap2D::worldToMapEnforceBounds(), so I'm using
|
||||
// -<float>::max() instead.
|
||||
*min_x = -std::numeric_limits<float>::max();
|
||||
*min_y = -std::numeric_limits<float>::max();
|
||||
*max_x = std::numeric_limits<float>::max();
|
||||
*max_y = std::numeric_limits<float>::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 (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<distance, list> to emulate the priority queue used before, with a notable performance boost
|
||||
|
||||
// Start with lethal obstacles: by definition distance is 0.0
|
||||
std::vector<CellData>& 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<double, std::vector<CellData> >::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
|
||||
623
navigations/costmap_2d/plugins/obstacle_layer.cpp
Executable file
623
navigations/costmap_2d/plugins/obstacle_layer.cpp
Executable file
@@ -0,0 +1,623 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include <tf2_ros/message_filter.h>
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
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());
|
||||
|
||||
// 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 (!(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<sensor_msgs::LaserScan>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
|
||||
|
||||
boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
|
||||
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
|
||||
if (inf_is_valid)
|
||||
{
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
|
||||
}
|
||||
else
|
||||
{
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
|
||||
}
|
||||
|
||||
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<sensor_msgs::PointCloud>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(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 < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
|
||||
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
|
||||
|
||||
observation_subscribers_.push_back(sub);
|
||||
observation_notifiers_.push_back(filter);
|
||||
}
|
||||
else
|
||||
{
|
||||
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(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 < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
|
||||
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
|
||||
|
||||
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<costmap_2d::ObstaclePluginConfig>(nh);
|
||||
dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
|
||||
[this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
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<ObservationBuffer>& 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 (tf2::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);
|
||||
}
|
||||
catch (std::runtime_error &ex)
|
||||
{
|
||||
ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
|
||||
return; //ignore this message
|
||||
}
|
||||
|
||||
// buffer the point cloud
|
||||
buffer->lock();
|
||||
buffer->bufferCloud(cloud);
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
|
||||
const boost::shared_ptr<ObservationBuffer>& 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 (tf2::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);
|
||||
}
|
||||
catch (std::runtime_error &ex)
|
||||
{
|
||||
ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
|
||||
return; //ignore this message
|
||||
}
|
||||
|
||||
// buffer the point cloud
|
||||
buffer->lock();
|
||||
buffer->bufferCloud(cloud);
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& 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<ObservationBuffer>& 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);
|
||||
useExtraBounds(min_x, min_y, max_x, max_y);
|
||||
|
||||
bool current = true;
|
||||
std::vector<Observation> 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<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
|
||||
{
|
||||
const Observation& obs = *it;
|
||||
|
||||
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
|
||||
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
|
||||
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
{
|
||||
double px = *iter_x, py = *iter_y, pz = *iter_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 (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<Observation>& 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<Observation>& 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;
|
||||
const sensor_msgs::PointCloud2 &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
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
|
||||
{
|
||||
double wx = *iter_x;
|
||||
double wy = *iter_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
|
||||
28
navigations/costmap_2d/plugins/preferred_layer.cpp
Executable file
28
navigations/costmap_2d/plugins/preferred_layer.cpp
Executable file
@@ -0,0 +1,28 @@
|
||||
#include <costmap_2d/preferred_layer.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_2d::PreferredLayer, costmap_2d::Layer)
|
||||
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::FREE_SPACE;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::PREFERRED_SPACE;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
PreferredLayer::PreferredLayer(){}
|
||||
PreferredLayer::~PreferredLayer(){}
|
||||
|
||||
unsigned char PreferredLayer::interpretValue(unsigned char value)
|
||||
{
|
||||
// check if the static value is above the unknown or lethal thresholds
|
||||
if(value == 0) return NO_INFORMATION;
|
||||
else if (value >= *this->threshold_)
|
||||
return PREFERRED_SPACE;
|
||||
|
||||
double scale = (double) value / *this->threshold_;
|
||||
return scale * LETHAL_OBSTACLE;
|
||||
}
|
||||
|
||||
}
|
||||
351
navigations/costmap_2d/plugins/static_layer.cpp
Executable file
351
navigations/costmap_2d/plugins/static_layer.cpp
Executable file
@@ -0,0 +1,351 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/static_layer.h>
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
|
||||
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)
|
||||
{
|
||||
threshold_ = &lethal_threshold_;
|
||||
}
|
||||
|
||||
StaticLayer::~StaticLayer()
|
||||
{
|
||||
if (dsrv_)
|
||||
delete dsrv_;
|
||||
}
|
||||
|
||||
void StaticLayer::onInitialize()
|
||||
{
|
||||
ros::NodeHandle nh("~/" + name_), g_nh;
|
||||
current_ = true;
|
||||
|
||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||
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);
|
||||
nh.param("base_frame_id", base_frame_id_, std::string("base_footprint"));
|
||||
|
||||
// ROS_WARN("global_frame_[%s] map_frame_[%s] map_topic[%s] first_map_only[%d] subscribe_to_updates[%d] track_unknown_space[%d] use_maximum[%d] \n lethal_cost_threshold[%d] unknown_cost_value[%d] trinary_costmap[%d]",
|
||||
// global_frame_.c_str(), map_frame_.c_str(),map_topic_.c_str(), first_map_only_, subscribe_to_updates_, track_unknown_space_, use_maximum_, temp_lethal_threshold, temp_unknown_cost_value, trinary_costmap_);
|
||||
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<costmap_2d::GenericPluginConfig>(nh);
|
||||
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb =
|
||||
[this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
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))
|
||||
{
|
||||
// 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 /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
|
||||
}
|
||||
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 (!layered_costmap_->isRolling())
|
||||
{
|
||||
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
|
||||
if (!use_maximum_)
|
||||
updateWithOverwrite(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
|
||||
geometry_msgs::TransformStamped transform;
|
||||
try
|
||||
{
|
||||
transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
|
||||
}
|
||||
catch (tf2::TransformException ex)
|
||||
{
|
||||
ROS_ERROR("%s", ex.what());
|
||||
return;
|
||||
}
|
||||
// Copy map data given proper transformations
|
||||
tf2::Transform tf2_transform;
|
||||
tf2::convert(transform.transform, tf2_transform);
|
||||
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_
|
||||
tf2::Vector3 p(wx, wy, 0);
|
||||
p = tf2_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
|
||||
27
navigations/costmap_2d/plugins/unpreferred_layer.cpp
Executable file
27
navigations/costmap_2d/plugins/unpreferred_layer.cpp
Executable file
@@ -0,0 +1,27 @@
|
||||
#include <costmap_2d/unpreferred_layer.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_2d::UnPreferredLayer, costmap_2d::Layer)
|
||||
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::PREFERRED_SPACE;
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
UnPreferredLayer::UnPreferredLayer(){}
|
||||
UnPreferredLayer::~UnPreferredLayer(){}
|
||||
|
||||
unsigned char UnPreferredLayer::interpretValue(unsigned char value)
|
||||
{
|
||||
// check if the static value is above the unknown or lethal thresholds
|
||||
if(value == 0) return NO_INFORMATION;
|
||||
else if (value >= *this->threshold_)
|
||||
return UNPREFERRED_SPACE;
|
||||
|
||||
double scale = (double) value / *this->threshold_;
|
||||
return scale * LETHAL_OBSTACLE;
|
||||
}
|
||||
|
||||
}
|
||||
449
navigations/costmap_2d/plugins/voxel_layer.cpp
Executable file
449
navigations/costmap_2d/plugins/voxel_layer.cpp
Executable file
@@ -0,0 +1,449 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/voxel_layer.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
#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<sensor_msgs::PointCloud>("clearing_endpoints", 1);
|
||||
}
|
||||
|
||||
void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
|
||||
{
|
||||
voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
|
||||
dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb =
|
||||
[this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
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);
|
||||
useExtraBounds(min_x, min_y, max_x, max_y);
|
||||
|
||||
bool current = true;
|
||||
std::vector<Observation> 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<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
|
||||
{
|
||||
const Observation& obs = *it;
|
||||
|
||||
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
|
||||
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
|
||||
for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
{
|
||||
// if the obstacle is too high or too far away from the robot we won't add it
|
||||
if (*iter_z > max_obstacle_height_)
|
||||
continue;
|
||||
|
||||
// compute the squared distance from the hitpoint to the pointcloud's origin
|
||||
double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
|
||||
+ (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
|
||||
+ (*iter_z - obs.origin_.z) * (*iter_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 (*iter_z < origin_z_)
|
||||
{
|
||||
if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz))
|
||||
continue;
|
||||
}
|
||||
else if (!worldToMap3D(*iter_x, *iter_y, *iter_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(*iter_x), double(*iter_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)
|
||||
{
|
||||
size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
|
||||
if (clearing_observation_cloud_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_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();
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
|
||||
|
||||
for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
{
|
||||
double wpx = *iter_x;
|
||||
double wpy = *iter_y;
|
||||
double wpz = *iter_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 = 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
|
||||
115
navigations/costmap_2d/src/array_parser.cpp
Executable file
115
navigations/costmap_2d/src/array_parser.cpp
Executable file
@@ -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 <cstdio> // for EOF
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
|
||||
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<std::vector<float> > parseVVF(const std::string& input, std::string& error_return)
|
||||
{
|
||||
std::vector<std::vector<float> > result;
|
||||
|
||||
std::stringstream input_ss(input);
|
||||
int depth = 0;
|
||||
std::vector<float> 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
|
||||
488
navigations/costmap_2d/src/costmap_2d.cpp
Executable file
488
navigations/costmap_2d/src/costmap_2d.cpp
Executable file
@@ -0,0 +1,488 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/costmap_2d.h>
|
||||
#include <cstdio>
|
||||
|
||||
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<mutex_t> lock(*access_);
|
||||
delete[] costmap_;
|
||||
costmap_ = NULL;
|
||||
}
|
||||
|
||||
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
|
||||
{
|
||||
boost::unique_lock<mutex_t> 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<mutex_t> 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<mutex_t> 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_);
|
||||
|
||||
// Nothing to update
|
||||
if (cell_ox == 0 && cell_oy == 0)
|
||||
return;
|
||||
|
||||
// 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<geometry_msgs::Point>& 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<MapLocation> 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<MapLocation> 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<MapLocation>& polygon, std::vector<MapLocation>& 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<MapLocation>& polygon, std::vector<MapLocation>& 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
|
||||
207
navigations/costmap_2d/src/costmap_2d_cloud.cpp
Executable file
207
navigations/costmap_2d/src/costmap_2d_cloud.cpp
Executable file
@@ -0,0 +1,207 @@
|
||||
/*
|
||||
* 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 <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <costmap_2d/VoxelGrid.h>
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
|
||||
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<Cell> 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<float*>(&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<float*>(&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, [&pub_marked,&pub_unknown](auto& msg){ voxelCallback(pub_marked, pub_unknown, msg); });
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
155
navigations/costmap_2d/src/costmap_2d_markers.cpp
Executable file
155
navigations/costmap_2d/src/costmap_2d_markers.cpp
Executable file
@@ -0,0 +1,155 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <costmap_2d/VoxelGrid.h>
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
|
||||
struct Cell
|
||||
{
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
voxel_grid::VoxelStatus status;
|
||||
};
|
||||
typedef std::vector<Cell> 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, [&pub](auto& msg){ voxelCallback(pub, msg); });
|
||||
g_marker_ns = n.resolveName("voxel_grid");
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
52
navigations/costmap_2d/src/costmap_2d_node.cpp
Executable file
52
navigations/costmap_2d/src/costmap_2d_node.cpp
Executable file
@@ -0,0 +1,52 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <ros/ros.h>
|
||||
#include <costmap_2d/costmap_2d_ros.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "costmap_node");
|
||||
tf2_ros::Buffer buffer(ros::Duration(10));
|
||||
tf2_ros::TransformListener tf(buffer);
|
||||
costmap_2d::Costmap2DROS lcr("costmap", buffer);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return (0);
|
||||
}
|
||||
182
navigations/costmap_2d/src/costmap_2d_publisher.cpp
Executable file
182
navigations/costmap_2d/src/costmap_2d_publisher.cpp
Executable file
@@ -0,0 +1,182 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <boost/bind.hpp>
|
||||
#include <costmap_2d/costmap_2d_publisher.h>
|
||||
#include <costmap_2d/cost_values.h>
|
||||
|
||||
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<nav_msgs::OccupancyGrid>(topic_name, 1,
|
||||
boost::bind(&Costmap2DPublisher::onNewSubscription, this, _1));
|
||||
costmap_update_pub_ = ros_node->advertise<map_msgs::OccupancyGridUpdate>(topic_name + "_updates", 1);
|
||||
|
||||
if (cost_translation_table_ == NULL)
|
||||
{
|
||||
cost_translation_table_ = new char[256];
|
||||
|
||||
// special values:
|
||||
cost_translation_table_[(const int)PREFERRED_SPACE] = 120; // Preferred place
|
||||
for(int i = (int)PREFERRED_SPACE+1; i <= (const int)FREE_SPACE; i++)
|
||||
cost_translation_table_[i] = 0; // NO obstacle
|
||||
|
||||
cost_translation_table_[(const int)CRITICAL_SPACE] = 150; // CRITICAL_SPACE
|
||||
cost_translation_table_[(const int)INSCRIBED_INFLATED_OBSTACLE] = 99; // INSCRIBED obstacle
|
||||
cost_translation_table_[(const int)LETHAL_OBSTACLE] = 100; // LETHAL obstacle
|
||||
cost_translation_table_[(const int)NO_INFORMATION] = -1; // UNKNOWN
|
||||
|
||||
// regular cost values scale the range 10 to 252 (inclusive) to fit
|
||||
// into 1 to 98 (inclusive).
|
||||
for (int i = (int)FREE_SPACE + 1; i < (const int)INSCRIBED_INFLATED_OBSTACLE; 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<Costmap2D::mutex_t> 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;
|
||||
}
|
||||
|
||||
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
|
||||
float resolution = costmap_->getResolution();
|
||||
// ROS_WARN("%s %d %d %d %d %d %d",
|
||||
// global_frame_.c_str(),
|
||||
// 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()
|
||||
// );
|
||||
|
||||
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_)
|
||||
{
|
||||
// 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
|
||||
619
navigations/costmap_2d/src/costmap_2d_ros.cpp
Executable file
619
navigations/costmap_2d/src/costmap_2d_ros.cpp
Executable file
@@ -0,0 +1,619 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/costmap_2d_ros.h>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
|
||||
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(const std::string& name, tf2_ros::Buffer& tf) :
|
||||
layered_costmap_(NULL),
|
||||
name_(name),
|
||||
tf_(tf),
|
||||
transform_tolerance_(0.3),
|
||||
map_update_thread_shutdown_(false),
|
||||
stop_updates_(false),
|
||||
initialized_(true),
|
||||
stopped_(false),
|
||||
map_update_thread_(NULL),
|
||||
last_publish_(0),
|
||||
plugin_loader_("costmap_2d", "costmap_2d::Layer"),
|
||||
publisher_(NULL),
|
||||
dsrv_(NULL),
|
||||
footprint_padding_(0.0)
|
||||
{
|
||||
|
||||
ros::NodeHandle private_nh("~/" + name);
|
||||
ros::NodeHandle g_nh;
|
||||
|
||||
// get global and robot base frame names
|
||||
private_nh.param("global_frame", global_frame_, std::string("map"));
|
||||
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
|
||||
// if(name_.find("global_costmap")) ROS_ERROR("Hiep- name_: %s global_frame: %s robot_base_frame: %s", name_.c_str(), global_frame_.c_str(), robot_base_frame_.c_str());
|
||||
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_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &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"))
|
||||
{
|
||||
loadOldParameters(private_nh);
|
||||
} else {
|
||||
warnForOldParameters(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<std::string>(my_list[i]["name"]);
|
||||
std::string type = static_cast<std::string>(my_list[i]["type"]);
|
||||
ROS_INFO("%s: Using plugin \"%s\"", name_.c_str(), pname.c_str());
|
||||
copyParentParameters(pname, type, private_nh);
|
||||
|
||||
boost::shared_ptr<Layer> 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("footprint")); // TODO: revert to oriented_footprint in N-turtle
|
||||
footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(topic, 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;
|
||||
|
||||
dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
|
||||
dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
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::loadOldParameters(ros::NodeHandle& nh)
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::loadOldParameters");
|
||||
ROS_WARN("%s: Parameter \"plugins\" not provided, loading pre-Hydro parameters", name_.c_str());
|
||||
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::copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh)
|
||||
{
|
||||
ros::NodeHandle target_layer(nh, plugin_name);
|
||||
|
||||
if(plugin_type == "costmap_2d::StaticLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "map_topic", false);
|
||||
move_parameter(nh, target_layer, "unknown_cost_value", false);
|
||||
move_parameter(nh, target_layer, "lethal_cost_threshold", false);
|
||||
move_parameter(nh, target_layer, "track_unknown_space", false);
|
||||
}
|
||||
else if(plugin_type == "costmap_2d::VoxelLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "origin_z", false);
|
||||
move_parameter(nh, target_layer, "z_resolution", false);
|
||||
move_parameter(nh, target_layer, "z_voxels", false);
|
||||
move_parameter(nh, target_layer, "mark_threshold", false);
|
||||
move_parameter(nh, target_layer, "unknown_threshold", false);
|
||||
move_parameter(nh, target_layer, "publish_voxel_map", false);
|
||||
}
|
||||
else if(plugin_type == "costmap_2d::ObstacleLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "max_obstacle_height", false);
|
||||
move_parameter(nh, target_layer, "raytrace_range", false);
|
||||
move_parameter(nh, target_layer, "obstacle_range", false);
|
||||
move_parameter(nh, target_layer, "track_unknown_space", false);
|
||||
}
|
||||
else if(plugin_type == "costmap_2d::InflationLayer")
|
||||
{
|
||||
move_parameter(nh, target_layer, "cost_scaling_factor", false);
|
||||
move_parameter(nh, target_layer, "inflation_radius", false);
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::warnForOldParameters(ros::NodeHandle& nh)
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::warnForOldParameters");
|
||||
checkOldParam(nh, "static_map");
|
||||
checkOldParam(nh, "map_type");
|
||||
}
|
||||
|
||||
void Costmap2DROS::checkOldParam(ros::NodeHandle& nh, const std::string ¶m_name){
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::checkOldParam");
|
||||
if(nh.hasParam(param_name)){
|
||||
ROS_WARN("%s: Pre-Hydro parameter \"%s\" unused since \"plugins\" is provided", name_.c_str(), param_name.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::reconfigureCB");
|
||||
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_ = NULL;
|
||||
}
|
||||
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;
|
||||
|
||||
// only construct the thread if the frequency is positive
|
||||
if(map_update_frequency > 0.0)
|
||||
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(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::readFootprintFromConfig");
|
||||
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<geometry_msgs::Point> 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<geometry_msgs::Point>& 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_);
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::movementCB");
|
||||
geometry_msgs::PoseStamped new_pose;
|
||||
|
||||
if (!getRobotPose(new_pose))
|
||||
{
|
||||
ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");
|
||||
}
|
||||
}
|
||||
|
||||
void Costmap2DROS::mapUpdateLoop(double frequency)
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
ros::Rate r(frequency);
|
||||
while (nh.ok() && !map_update_thread_shutdown_)
|
||||
{
|
||||
#ifdef HAVE_SYS_TIME_H
|
||||
struct timeval start, end;
|
||||
double start_t, end_t, t_diff;
|
||||
gettimeofday(&start, NULL);
|
||||
#endif
|
||||
|
||||
updateMap();
|
||||
|
||||
#ifdef HAVE_SYS_TIME_H
|
||||
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);
|
||||
#endif
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
ros::spinOnce();
|
||||
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
|
||||
geometry_msgs::PoseStamped pose;
|
||||
if (getRobotPose (pose))
|
||||
{
|
||||
double x = pose.pose.position.x,
|
||||
y = pose.pose.position.y,
|
||||
yaw = tf2::getYaw(pose.pose.orientation);
|
||||
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()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::start");
|
||||
std::vector < boost::shared_ptr<Layer> > *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<boost::shared_ptr<Layer> >::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
|
||||
// note: this does not hold, if the user has disabled map-updates allgother
|
||||
ros::Rate r(100.0);
|
||||
while (ros::ok() && !initialized_ && map_update_thread_)
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
void Costmap2DROS::stop()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::stop");
|
||||
stop_updates_ = true;
|
||||
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
|
||||
// unsubscribe from topics
|
||||
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->deactivate();
|
||||
}
|
||||
initialized_ = false;
|
||||
stopped_ = true;
|
||||
}
|
||||
|
||||
void Costmap2DROS::pause()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::pause");
|
||||
stop_updates_ = true;
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
void Costmap2DROS::resume()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("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()
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::resetLayers");
|
||||
Costmap2D* top = layered_costmap_->getCostmap();
|
||||
top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
|
||||
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
|
||||
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->reset();
|
||||
}
|
||||
}
|
||||
|
||||
bool Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getRobotPose");
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
|
||||
geometry_msgs::PoseStamped robot_pose;
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
|
||||
robot_pose.header.frame_id = robot_base_frame_;
|
||||
robot_pose.header.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
|
||||
{
|
||||
// use current time if possible (makes sure it's not in the future)
|
||||
if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
|
||||
{
|
||||
geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
|
||||
tf2::doTransform(robot_pose, global_pose, transform);
|
||||
}
|
||||
// use the latest otherwise
|
||||
else
|
||||
{
|
||||
tf_.transform(robot_pose, global_pose, global_frame_);
|
||||
}
|
||||
}
|
||||
catch (tf2::LookupException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ConnectivityException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ExtrapolationException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
// check global_pose timeout
|
||||
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.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.header.stamp.toSec(), transform_tolerance_);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
|
||||
{
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getOrientedFootprint");
|
||||
geometry_msgs::PoseStamped global_pose;
|
||||
if (!getRobotPose(global_pose))
|
||||
return;
|
||||
|
||||
double yaw = tf2::getYaw(global_pose.pose.orientation);
|
||||
transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
|
||||
padded_footprint_, oriented_footprint);
|
||||
}
|
||||
|
||||
} // namespace costmap_2d
|
||||
173
navigations/costmap_2d/src/costmap_layer.cpp
Executable file
173
navigations/costmap_2d/src/costmap_layer.cpp
Executable file
@@ -0,0 +1,173 @@
|
||||
#include<costmap_2d/costmap_layer.h>
|
||||
|
||||
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::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
|
||||
{
|
||||
unsigned char* grid = getCharMap();
|
||||
for(int x=0; x<(int)getSizeInCellsX(); x++){
|
||||
bool xrange = x>start_x && x<end_x;
|
||||
|
||||
for(int y=0; y<(int)getSizeInCellsY(); y++){
|
||||
if((xrange && y>start_y && y<end_y)!=invert_area)
|
||||
continue;
|
||||
int index = getIndex(x,y);
|
||||
if(grid[index]!=NO_INFORMATION){
|
||||
grid[index] = NO_INFORMATION;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
if (costmap_[it] == CRITICAL_SPACE){
|
||||
master_array[it] = costmap_[it];
|
||||
it++;
|
||||
continue;
|
||||
}
|
||||
if (costmap_[it] == PREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
|
||||
master_array[it] = costmap_[it];
|
||||
it++;
|
||||
continue;
|
||||
}
|
||||
if (costmap_[it] == UNPREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
|
||||
master_array[it] = costmap_[it];
|
||||
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
|
||||
89
navigations/costmap_2d/src/costmap_math.cpp
Executable file
89
navigations/costmap_2d/src/costmap_math.cpp
Executable file
@@ -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 <costmap_2d/costmap_math.h>
|
||||
|
||||
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<geometry_msgs::Point>& 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<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& 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<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
|
||||
{
|
||||
return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
|
||||
}
|
||||
325
navigations/costmap_2d/src/footprint.cpp
Executable file
325
navigations/costmap_2d/src/footprint.cpp
Executable file
@@ -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<costmap_2d/costmap_math.h>
|
||||
#include <boost/tokenizer.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <costmap_2d/array_parser.h>
|
||||
#include<geometry_msgs/Point32.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
|
||||
{
|
||||
min_dist = std::numeric_limits<double>::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<geometry_msgs::Point> pts)
|
||||
{
|
||||
geometry_msgs::Polygon polygon;
|
||||
for (int i = 0; i < pts.size(); i++){
|
||||
polygon.points.push_back(toPoint32(pts[i]));
|
||||
}
|
||||
return polygon;
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
|
||||
{
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point>& footprint_spec,
|
||||
std::vector<geometry_msgs::Point>& 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<geometry_msgs::Point>& 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<geometry_msgs::Point>& 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<geometry_msgs::Point> makeFootprintFromRadius(double radius)
|
||||
{
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point>& footprint)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > 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<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
|
||||
{
|
||||
std::string full_param_name;
|
||||
std::string full_radius_param_name;
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point>& 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<geometry_msgs::Point> 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<geometry_msgs::Point> 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
|
||||
56
navigations/costmap_2d/src/layer.cpp
Executable file
56
navigations/costmap_2d/src/layer.cpp
Executable file
@@ -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, tf2_ros::Buffer *tf)
|
||||
{
|
||||
layered_costmap_ = parent;
|
||||
name_ = name;
|
||||
tf_ = tf;
|
||||
onInitialize();
|
||||
}
|
||||
|
||||
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
|
||||
{
|
||||
return layered_costmap_->getFootprint();
|
||||
}
|
||||
|
||||
} // end namespace costmap_2d
|
||||
190
navigations/costmap_2d/src/layered_costmap.cpp
Executable file
190
navigations/costmap_2d/src/layered_costmap.cpp
Executable file
@@ -0,0 +1,190 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
|
||||
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(NO_INFORMATION);
|
||||
else
|
||||
costmap_.setDefaultValue(FREE_SPACE);
|
||||
}
|
||||
|
||||
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<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
|
||||
size_locked_ = size_locked;
|
||||
costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
|
||||
for (vector<boost::shared_ptr<Layer>>::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<Costmap2D::mutex_t> 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<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
||||
++plugin)
|
||||
{
|
||||
if (!(*plugin)->isEnabled())
|
||||
continue;
|
||||
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<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
||||
++plugin)
|
||||
{
|
||||
if ((*plugin)->isEnabled())
|
||||
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
|
||||
}
|
||||
|
||||
bx0_ = x0;
|
||||
bxn_ = xn;
|
||||
by0_ = y0;
|
||||
byn_ = yn;
|
||||
|
||||
initialized_ = true;
|
||||
}
|
||||
|
||||
bool LayeredCostmap::isCurrent()
|
||||
{
|
||||
current_ = true;
|
||||
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
||||
++plugin)
|
||||
{
|
||||
if ((*plugin)->isEnabled())
|
||||
current_ = current_ && (*plugin)->isCurrent();
|
||||
}
|
||||
return current_;
|
||||
}
|
||||
|
||||
void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec)
|
||||
{
|
||||
footprint_ = footprint_spec;
|
||||
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
|
||||
|
||||
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
||||
++plugin)
|
||||
{
|
||||
(*plugin)->onFootprintChanged();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace costmap_2d
|
||||
251
navigations/costmap_2d/src/observation_buffer.cpp
Executable file
251
navigations/costmap_2d/src/observation_buffer.cpp
Executable file
@@ -0,0 +1,251 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <costmap_2d/observation_buffer.h>
|
||||
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace tf2;
|
||||
|
||||
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, tf2_ros::Buffer& tf2_buffer, string global_frame,
|
||||
string sensor_frame, double tf_tolerance) :
|
||||
tf2_buffer_(tf2_buffer), 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;
|
||||
|
||||
geometry_msgs::TransformStamped transformStamped;
|
||||
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), &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<Observation>::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
|
||||
tf2_buffer_.transform(origin, origin, new_global_frame);
|
||||
obs.origin_ = origin.point;
|
||||
|
||||
// we also need to transform the cloud of the observation to the new global frame
|
||||
tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
|
||||
}
|
||||
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)
|
||||
{
|
||||
geometry_msgs::PointStamped 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
|
||||
geometry_msgs::PointStamped local_origin;
|
||||
local_origin.header.stamp = cloud.header.stamp;
|
||||
local_origin.header.frame_id = origin_frame;
|
||||
local_origin.point.x = 0;
|
||||
local_origin.point.y = 0;
|
||||
local_origin.point.z = 0;
|
||||
tf2_buffer_.transform(local_origin, global_origin, global_frame_);
|
||||
tf2::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
||||
// 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_;
|
||||
|
||||
sensor_msgs::PointCloud2 global_frame_cloud;
|
||||
|
||||
// transform the point cloud
|
||||
tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
|
||||
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
|
||||
sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
|
||||
observation_cloud.height = global_frame_cloud.height;
|
||||
observation_cloud.width = global_frame_cloud.width;
|
||||
observation_cloud.fields = global_frame_cloud.fields;
|
||||
observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
|
||||
observation_cloud.point_step = global_frame_cloud.point_step;
|
||||
observation_cloud.row_step = global_frame_cloud.row_step;
|
||||
observation_cloud.is_dense = global_frame_cloud.is_dense;
|
||||
|
||||
unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
|
||||
sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
|
||||
modifier.resize(cloud_size);
|
||||
unsigned int point_count = 0;
|
||||
|
||||
// copy over the points that are within our height bounds
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
|
||||
std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
|
||||
std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
|
||||
for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
|
||||
{
|
||||
if ((*iter_z) <= max_obstacle_height_
|
||||
&& (*iter_z) >= min_obstacle_height_)
|
||||
{
|
||||
std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
|
||||
iter_obs += global_frame_cloud.point_step;
|
||||
++point_count;
|
||||
}
|
||||
}
|
||||
|
||||
// resize the cloud for the number of legal points
|
||||
modifier.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<Observation>& 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<Observation>::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<Observation>::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
|
||||
if ((last_updated_ - 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
|
||||
|
||||
BIN
navigations/costmap_2d/test/TenByTen.pgm
Executable file
BIN
navigations/costmap_2d/test/TenByTen.pgm
Executable file
Binary file not shown.
7
navigations/costmap_2d/test/TenByTen.yaml
Executable file
7
navigations/costmap_2d/test/TenByTen.yaml
Executable file
@@ -0,0 +1,7 @@
|
||||
image: TenByTen.pgm
|
||||
resolution: 1.0
|
||||
origin: [0,0,0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
80
navigations/costmap_2d/test/array_parser_test.cpp
Executable file
80
navigations/costmap_2d/test/array_parser_test.cpp
Executable file
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "costmap_2d/array_parser.h"
|
||||
|
||||
using namespace costmap_2d;
|
||||
|
||||
TEST(array_parser, basic_operation)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > 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<std::vector<float> > vvf;
|
||||
vvf = parseVVF( "[1, 2.2], [.3, -4e4]]", error );
|
||||
EXPECT_FALSE( error == "" );
|
||||
}
|
||||
|
||||
TEST(array_parser, missing_close)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > vvf;
|
||||
vvf = parseVVF( "[[1, 2.2], [.3, -4e4]", error );
|
||||
EXPECT_FALSE( error == "" );
|
||||
}
|
||||
|
||||
TEST(array_parser, wrong_depth)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > 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();
|
||||
}
|
||||
|
||||
132
navigations/costmap_2d/test/coordinates_test.cpp
Executable file
132
navigations/costmap_2d/test/coordinates_test.cpp
Executable file
@@ -0,0 +1,132 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2019, Locus Robotics
|
||||
* 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 copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
// Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
|
||||
using namespace costmap_2d;
|
||||
|
||||
TEST(CostmapCoordinates, easy_coordinates_test)
|
||||
{
|
||||
Costmap2D costmap(2, 3, 1.0, 0, 0);
|
||||
|
||||
double wx, wy;
|
||||
costmap.mapToWorld(0u, 0u, wx, wy);
|
||||
EXPECT_DOUBLE_EQ(wx, 0.5);
|
||||
EXPECT_DOUBLE_EQ(wy, 0.5);
|
||||
costmap.mapToWorld(1u, 2u, wx, wy);
|
||||
EXPECT_DOUBLE_EQ(wx, 1.5);
|
||||
EXPECT_DOUBLE_EQ(wy, 2.5);
|
||||
|
||||
unsigned int umx, umy;
|
||||
int mx, my;
|
||||
ASSERT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 1);
|
||||
EXPECT_EQ(umy, 2);
|
||||
costmap.worldToMapNoBounds(wx, wy, mx, my);
|
||||
EXPECT_EQ(mx, 1);
|
||||
EXPECT_EQ(my, 2);
|
||||
|
||||
// Invalid Coordinate
|
||||
wx = 2.5;
|
||||
EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
|
||||
costmap.worldToMapEnforceBounds(wx, wy, mx, my);
|
||||
EXPECT_EQ(mx, 1);
|
||||
EXPECT_EQ(my, 2);
|
||||
costmap.worldToMapNoBounds(wx, wy, mx, my);
|
||||
EXPECT_EQ(mx, 2);
|
||||
EXPECT_EQ(my, 2);
|
||||
|
||||
// Border Cases
|
||||
EXPECT_TRUE(costmap.worldToMap(0.0, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 0);
|
||||
EXPECT_TRUE(costmap.worldToMap(0.25, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 0);
|
||||
EXPECT_TRUE(costmap.worldToMap(0.75, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 0);
|
||||
EXPECT_TRUE(costmap.worldToMap(0.9999, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 0);
|
||||
EXPECT_TRUE(costmap.worldToMap(1.0, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 1);
|
||||
EXPECT_TRUE(costmap.worldToMap(1.25, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 1);
|
||||
EXPECT_TRUE(costmap.worldToMap(1.75, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 1);
|
||||
EXPECT_TRUE(costmap.worldToMap(1.9999, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 1);
|
||||
EXPECT_FALSE(costmap.worldToMap(2.0, wy, umx, umy));
|
||||
costmap.worldToMapEnforceBounds(2.0, wy, mx, my);
|
||||
EXPECT_EQ(mx, 1);
|
||||
}
|
||||
|
||||
TEST(CostmapCoordinates, hard_coordinates_test)
|
||||
{
|
||||
Costmap2D costmap(2, 3, 0.1, -0.2, 0.2);
|
||||
|
||||
double wx, wy;
|
||||
costmap.mapToWorld(0, 0, wx, wy);
|
||||
EXPECT_DOUBLE_EQ(wx, -0.15);
|
||||
EXPECT_DOUBLE_EQ(wy, 0.25);
|
||||
costmap.mapToWorld(1, 2, wx, wy);
|
||||
EXPECT_DOUBLE_EQ(wx, -0.05);
|
||||
EXPECT_DOUBLE_EQ(wy, 0.45);
|
||||
|
||||
unsigned int umx, umy;
|
||||
int mx, my;
|
||||
EXPECT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
|
||||
EXPECT_EQ(umx, 1);
|
||||
EXPECT_EQ(umy, 2);
|
||||
costmap.worldToMapNoBounds(wx, wy, mx, my);
|
||||
EXPECT_EQ(mx, 1);
|
||||
EXPECT_EQ(my, 2);
|
||||
|
||||
// Invalid Coordinate
|
||||
wx = 2.5;
|
||||
EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
|
||||
costmap.worldToMapEnforceBounds(wx, wy, mx, my);
|
||||
EXPECT_EQ(mx, 1);
|
||||
EXPECT_EQ(my, 2);
|
||||
costmap.worldToMapNoBounds(wx, wy, mx, my);
|
||||
EXPECT_EQ(mx, 27);
|
||||
EXPECT_EQ(my, 2);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest( &argc, argv );
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
||||
29
navigations/costmap_2d/test/costmap_params.yaml
Executable file
29
navigations/costmap_2d/test/costmap_params.yaml
Executable file
@@ -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}
|
||||
146
navigations/costmap_2d/test/costmap_tester.cpp
Executable file
146
navigations/costmap_2d/test/costmap_tester.cpp
Executable file
@@ -0,0 +1,146 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <gtest/gtest.h>
|
||||
#include <ros/ros.h>
|
||||
#include <costmap_2d/costmap_2d_ros.h>
|
||||
#include <costmap_2d/cost_values.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
namespace costmap_2d {
|
||||
|
||||
class CostmapTester : public testing::Test {
|
||||
public:
|
||||
CostmapTester(tf2_ros::Buffer& 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(tf2_ros::Buffer& 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<int>(x-nx), static_cast<int>(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("~");
|
||||
|
||||
tf2_ros::Buffer tf(ros::Duration(10));
|
||||
tf2_ros::TransformListener tfl(tf);
|
||||
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);
|
||||
}
|
||||
182
navigations/costmap_2d/test/footprint_tests.cpp
Executable file
182
navigations/costmap_2d/test/footprint_tests.cpp
Executable file
@@ -0,0 +1,182 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 <gtest/gtest.h>
|
||||
#include <ros/ros.h>
|
||||
#include <costmap_2d/costmap_2d_ros.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
using namespace costmap_2d;
|
||||
|
||||
tf2_ros::TransformListener* tfl_;
|
||||
tf2_ros::Buffer* tf_;
|
||||
|
||||
TEST( Costmap2DROS, unpadded_footprint_from_string_param )
|
||||
{
|
||||
Costmap2DROS cm( "unpadded/string", *tf_ );
|
||||
std::vector<geometry_msgs::Point> 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<geometry_msgs::Point> 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<geometry_msgs::Point> 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<geometry_msgs::Point> 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<geometry_msgs::Point> 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<geometry_msgs::Point> 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 tf2_ros::Buffer( ros::Duration( 10 ));
|
||||
tfl_ = new tf2_ros::TransformListener(*tf_);
|
||||
|
||||
// This empty transform is added to satisfy the constructor of
|
||||
// Costmap2DROS, which waits for the transform from map to base_link
|
||||
// to become available.
|
||||
geometry_msgs::TransformStamped base_rel_map;
|
||||
base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
|
||||
base_rel_map.child_frame_id = "base_link";
|
||||
base_rel_map.header.frame_id = "map";
|
||||
base_rel_map.header.stamp = ros::Time::now();
|
||||
tf_->setTransform( base_rel_map, "footprint_tests" );
|
||||
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
29
navigations/costmap_2d/test/footprint_tests.launch
Executable file
29
navigations/costmap_2d/test/footprint_tests.launch
Executable file
@@ -0,0 +1,29 @@
|
||||
<launch>
|
||||
|
||||
<test time-limit="10" test-name="footprint_test" pkg="costmap_2d" type="footprint_tests">
|
||||
<param name="unpadded/string/footprint_padding" value="0" />
|
||||
<param name="unpadded/footprint" value="[[1, 1], [-1, 1], [-1, -1]]" />
|
||||
|
||||
<param name="padded/string/footprint_padding" value=".5" />
|
||||
<param name="padded/footprint" value="[[1, 1], [-1, 1], [-1, -1]]" />
|
||||
|
||||
<param name="radius/sub/footprint_padding" value="0" />
|
||||
<param name="radius/robot_radius" value="10" />
|
||||
|
||||
<rosparam ns="xmlrpc">
|
||||
footprint_padding: 0
|
||||
footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]]
|
||||
</rosparam>
|
||||
|
||||
<rosparam ns="xmlrpc_fail"> <!-- Footprint includes a 3-value point, which should make it fail. -->
|
||||
footprint_padding: 0
|
||||
footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]]
|
||||
</rosparam>
|
||||
|
||||
<param name="same_level/footprint_padding" value="0" />
|
||||
<param name="same_level/footprint" value="[[1, 2], [3, 4], [5, 6]]" />
|
||||
|
||||
<param name="empty/empty" value="0" /> <!-- just so you can see there are no real params under "empty". -->
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
417
navigations/costmap_2d/test/inflation_tests.cpp
Executable file
417
navigations/costmap_2d/test/inflation_tests.cpp
Executable file
@@ -0,0 +1,417 @@
|
||||
/*
|
||||
* 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 <map>
|
||||
#include <cmath>
|
||||
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/inflation_layer.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/testing_helper.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace costmap_2d;
|
||||
using geometry_msgs::Point;
|
||||
|
||||
std::vector<Point> setRadii(LayeredCostmap& layers, double length, double width, double inflation_radius)
|
||||
{
|
||||
std::vector<Point> 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<double, std::vector<CellData> > m;
|
||||
CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
|
||||
m[0].push_back(initial);
|
||||
for (std::map<double, std::vector<CellData> >::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 (dist == bin->first)
|
||||
{
|
||||
// Adding to our current bin could cause a reallocation
|
||||
// Which appears to cause the iterator to get messed up
|
||||
dist += 0.001;
|
||||
}
|
||||
|
||||
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){
|
||||
tf2_ros::Buffer 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<Point> 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){
|
||||
tf2_ros::Buffer 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<Point> 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){
|
||||
tf2_ros::Buffer 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<Point> 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<Observation> 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){
|
||||
tf2_ros::Buffer 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<Point> 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){
|
||||
|
||||
tf2_ros::Buffer tf;
|
||||
LayeredCostmap layers("frame", false, false);
|
||||
|
||||
// Footprint with inscribed radius = 2.1
|
||||
// circumscribed radius = 3.1
|
||||
std::vector<Point> 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<unsigned int>::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){
|
||||
|
||||
tf2_ros::Buffer tf;
|
||||
LayeredCostmap layers("frame", false, false);
|
||||
|
||||
// Footprint with inscribed radius = 2.1
|
||||
// circumscribed radius = 3.1
|
||||
std::vector<Point> 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){
|
||||
tf2_ros::Buffer tf;
|
||||
LayeredCostmap layers("frame", false, false);
|
||||
layers.resizeMap(10, 10, 1, 0, 0);
|
||||
|
||||
// 1 2 3
|
||||
std::vector<Point> 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();
|
||||
}
|
||||
6
navigations/costmap_2d/test/inflation_tests.launch
Executable file
6
navigations/costmap_2d/test/inflation_tests.launch
Executable file
@@ -0,0 +1,6 @@
|
||||
<launch>
|
||||
<node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
|
||||
<test time-limit="300" test-name="inflation_tests" pkg="costmap_2d" type="inflation_tests">
|
||||
<param name="inflation/cost_scaling_factor" value="1" />
|
||||
</test>
|
||||
</launch>
|
||||
1176
navigations/costmap_2d/test/module_tests.cpp
Executable file
1176
navigations/costmap_2d/test/module_tests.cpp
Executable file
File diff suppressed because it is too large
Load Diff
251
navigations/costmap_2d/test/obstacle_tests.cpp
Executable file
251
navigations/costmap_2d/test/obstacle_tests.cpp
Executable file
@@ -0,0 +1,251 @@
|
||||
/*
|
||||
* 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 <costmap_2d/costmap_2d.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/testing_helper.h>
|
||||
#include <set>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
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){
|
||||
tf2_ros::Buffer 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){
|
||||
tf2_ros::Buffer 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){
|
||||
tf2_ros::Buffer 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){
|
||||
tf2_ros::Buffer 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){
|
||||
tf2_ros::Buffer 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){
|
||||
tf2_ros::Buffer 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();
|
||||
}
|
||||
5
navigations/costmap_2d/test/obstacle_tests.launch
Executable file
5
navigations/costmap_2d/test/obstacle_tests.launch
Executable file
@@ -0,0 +1,5 @@
|
||||
<launch>
|
||||
<node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
|
||||
<test time-limit="300" test-name="obstacle_tests" pkg="costmap_2d" type="obstacle_tests" />
|
||||
|
||||
</launch>
|
||||
14
navigations/costmap_2d/test/simple_driving_test.xml
Executable file
14
navigations/costmap_2d/test/simple_driving_test.xml
Executable file
@@ -0,0 +1,14 @@
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
|
||||
<node name="rosplay" pkg="rosbag" type="play"
|
||||
args="-s 5 -r 1 --clock --hz=10 $(find costmap_2d)/test/simple_driving_test_indexed.bag" />
|
||||
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/willow-full-0.025.pgm 0.025" />
|
||||
|
||||
<rosparam file="$(find costmap_2d)/test/costmap_params.yaml" command="load" ns="simple_driving_test/test_costmap" />
|
||||
<test time-limit="600" test-name="simple_driving_test" pkg="costmap_2d" type="costmap_tester">
|
||||
<param name="wait_time" value="40.0" />
|
||||
</test>
|
||||
|
||||
</launch>
|
||||
330
navigations/costmap_2d/test/static_tests.cpp
Executable file
330
navigations/costmap_2d/test/static_tests.cpp
Executable file
@@ -0,0 +1,330 @@
|
||||
/*
|
||||
* 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 <costmap_2d/costmap_2d.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/static_layer.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/testing_helper.h>
|
||||
#include <set>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace costmap_2d;
|
||||
|
||||
|
||||
/**
|
||||
* Tests the reset method
|
||||
*
|
||||
TEST(costmap, testResetForStaticMap){
|
||||
// Define a static map with a large object in the center
|
||||
std::vector<unsigned char> 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<pcl::PointXYZ> 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<Observation> 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<unsigned char> 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<unsigned char> 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<unsigned int> 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<unsigned int>::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();
|
||||
}
|
||||
5
navigations/costmap_2d/test/static_tests.launch
Executable file
5
navigations/costmap_2d/test/static_tests.launch
Executable file
@@ -0,0 +1,5 @@
|
||||
<launch>
|
||||
<node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
|
||||
<test time-limit="300" test-name="static_tests" pkg="costmap_2d" type="static_tests" />
|
||||
|
||||
</launch>
|
||||
Reference in New Issue
Block a user