HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
navigations/.vscode/settings.json vendored Executable file
View File

@@ -0,0 +1,20 @@
{
"files.associations": {
"iterator": "cpp",
"cmath": "cpp",
"complex": "cpp",
"*.ipp": "cpp",
"array": "cpp",
"string_view": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"random": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"functional": "cpp",
"bitset": "cpp",
"initializer_list": "cpp",
"utility": "cpp",
"list": "cpp"
}
}

258
navigations/amcl/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,258 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package amcl
^^^^^^^^^^^^^^^^^^^^^^^^^^
1.17.3 (2023-01-10)
-------------------
* [AMCL] Add option to force nomotion update after initialpose (`#1226 <https://github.com/ros-planning/navigation/issues/1226>`_)
* Adds a new boolean parameter force_update_after_initialpose. When set to true, an update is forced on the next laser scan callback, such as when the /request_nomotion_update service is called. This often results in an improved robot pose after a manual (not very precise) re-localization - without a need for the robot to move.
* Fixes a bunch of compiler warnings (unused variable now, catching exceptions by value), normalizes how tf exceptions are caught
* [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, Stephan
1.17.2 (2022-06-20)
-------------------
* Update pf.c (`#1161 <https://github.com/ros-planning/navigation/issues/1161>`_)
`#1160 <https://github.com/ros-planning/navigation/issues/1160>`_ AMCL miscalculates orientation covariance for clusters
* Improved Overall readablity (`#1177 <https://github.com/ros-planning/navigation/issues/1177>`_)
* 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
* Fixes `#1117 <https://github.com/ros-planning/navigation/issues/1117>`_ (`#1118 <https://github.com/ros-planning/navigation/issues/1118>`_)
* Fixed the risk of divide by zero. (`#1099 <https://github.com/ros-planning/navigation/issues/1099>`_)
* (AMCL) add missing test dep on tf2_py (`#1091 <https://github.com/ros-planning/navigation/issues/1091>`_)
* (AMCL)(Noetic) use robot pose in tests (`#1087 <https://github.com/ros-planning/navigation/issues/1087>`_)
* (amcl) fix missing '#if NEW_UNIFORM_SAMPLING' (`#1079 <https://github.com/ros-planning/navigation/issues/1079>`_)
* Contributors: David V. Lu!!, Matthijs van der Burgh, Noriaki Ando, Supernovae, christofschroeter, easylyou
1.17.1 (2020-08-27)
-------------------
* (AMCL) add resample limit cache [Noetic] (`#1014 <https://github.com/ros-planning/navigation/issues/1014>`_)
* Contributors: Matthijs van der Burgh
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
* map is not subscriptable in python3
* fix python3 errors in basic_localization.py
* use upstream pykdl
* 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)
-------------------
* Implement selective resampling (`#921 <https://github.com/cobalt-robotics/navigation/issues/921>`_) (`#971 <https://github.com/cobalt-robotics/navigation/issues/971>`_)
Co-authored-by: Adi Vardi <adidasv111@gmail.com>
* Add CLI option to trigger global localization before processing a bagfile (`#816 <https://github.com/cobalt-robotics/navigation/issues/816>`_) (`#970 <https://github.com/cobalt-robotics/navigation/issues/970>`_)
Co-authored-by: alain-m <alain@savioke.com>
* Fix some reconfigure parameters not being applied [amcl]. (`#952 <https://github.com/cobalt-robotics/navigation/issues/952>`_)
* amcl: include missing CMake functions to fix build (`#946 <https://github.com/cobalt-robotics/navigation/issues/946>`_)
* Set proper limits for the z-weights [amcl]. (`#953 <https://github.com/cobalt-robotics/navigation/issues/953>`_)
* Merge pull request `#965 <https://github.com/cobalt-robotics/navigation/issues/965>`_ from nlimpert/nlimpert/fix_missing_cmake_include
Add missing CMake include(CheckSymbolExists) for CMake >= 3.15
* amcl: add missing CMake include(CheckSymbolExists)
Starting with CMake 3.15 an explicit include(CheckSymbolExists)
is required to use the check_symbol_exists macro.
* Contributors: Ben Wolsieffer, Michael Ferguson, Nicolas Limpert, Patrick Chin
1.16.3 (2019-11-15)
-------------------
* Merge branch 'melodic-devel' into layer_clear_area-melodic
* Fix typo in amcl_laser model header (`#918 <https://github.com/ros-planning/navigation/issues/918>`_)
* Merge pull request `#849 <https://github.com/ros-planning/navigation/issues/849>`_ from seanyen/amcl_windows_fix
[Windows][melodic] AMCL Windows build bring up.
* revert unrelated changes.
* AMCL windows build bring up.
* Add HAVE_UNISTD and HAVE_DRAND48 and portable_utils.hpp for better cross compiling.
* Variable length array is not supported in MSVC, conditionally disable it.
* Fix install location for shared lib and executables on Windows.
* Use isfinite for better cross compiling.
* feat: AMCL Diagnostics (`#807 <https://github.com/ros-planning/navigation/issues/807>`_)
Diagnostic task that monitors the estimated standard deviation of the filter.
By: reinzor <reinzor@gmail.com>
* fix typo for parameter beam_skip_error_threshold but bandaged for other users in AMCL (`#790 <https://github.com/ros-planning/navigation/issues/790>`_)
* fix typo but bandage for other users
* Merge pull request `#785 <https://github.com/ros-planning/navigation/issues/785>`_ from mintar/amcl_c++11
amcl: Add compile option C++11
* amcl: Set C++ standard 11 if not set
This is required to build the melodic-devel branch of the navigation
stack on kinetic. Melodic sets CMAKE_CXX_STANDARD=14, but kinetic
doesn't set that variable at all.
* Contributors: Hadi Tabatabaee, Martin Günther, Michael Ferguson, Rein Appeldoorn, Sean Yen, Steven Macenski
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 amcl to have proper depends
* add geometry_msgs
* add tf2_msgs
* fix alphabetical order
* 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>`_)
* make AMCL depend on sensor_msgs
previously, amcl depended on TF, which depended on
sensor_msgs.
* Contributors: Michael Ferguson
1.16.0 (2018-07-25)
-------------------
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Merge pull request `#734 <https://github.com/ros-planning/navigation/issues/734>`_ from ros-planning/melodic_731
AMCL dynamic reconfigure: Extend parameter range (Forward port `#731 <https://github.com/ros-planning/navigation/issues/731>`_)
* Merge pull request `#728 <https://github.com/ros-planning/navigation/issues/728>`_ from ros-planning/melodic_tf2_conversion
switch AMCL to use TF2
* fix swapped odom1/4 in omni model, fixes `#499 <https://github.com/ros-planning/navigation/issues/499>`_
* Merge pull request `#730 <https://github.com/ros-planning/navigation/issues/730>`_ from Glowcloud/melodic-devel
Fix for Potential Memory Leak in AmclNode::reconfigureCB `#729 <https://github.com/ros-planning/navigation/issues/729>`_
* Fix for Potential Memory Leak in AmclNode::reconfigureCB
* switch AMCL to use TF2
* Merge pull request `#727 <https://github.com/ros-planning/navigation/issues/727>`_ from ros-planning/melodic_668
Update laser_model_type enum on AMCL.cfg (Melodic port of `#668 <https://github.com/ros-planning/navigation/issues/668>`_)
* Update laser_model_type enum on AMCL.cfg
Adding likelihood_field_prob laser model option on AMCL.cfg to be able to control dynamic parameters with this laser sensor model.
* Merge pull request `#723 <https://github.com/ros-planning/navigation/issues/723>`_ from moriarty/melodic-buildfarm-errors
Melodic buildfarm errors
* include <memory> for std::shared_ptr
* Merge pull request `#718 <https://github.com/ros-planning/navigation/issues/718>`_ from moriarty/tf2-buffer-ptr
[melodic] tf2_buffer\_ -> tf2_buffer_ptr\_
* [melodic] tf2_buffer\_ -> tf2_buffer_ptr\_
Change required due to changes in upstream dependencies:
`ros/geometry#163 <https://github.com/ros/geometry/issues/163>`_: "Maintain & expose tf2 Buffer in shared_ptr for tf"
fixes `ros-planning/navigation#717 <https://github.com/ros-planning/navigation/issues/717>`_ (for compile errors at least.)
* Contributors: Alexander Moriarty, Glowcloud, Martin Ganeff, Michael Ferguson, Miguel Cordero, Vincent Rabaud, maracuya-robotics
1.15.2 (2018-03-22)
-------------------
* Fix minor typo (`#682 <https://github.com/ros-planning/navigation/issues/682>`_)
This typo caused some confusion because we were searching for a semicolon in our configuration.
* Merge pull request `#677 <https://github.com/ros-planning/navigation/issues/677>`_ from ros-planning/lunar_634
removing recomputation of cluster stats causing assertion error (`#634 <https://github.com/ros-planning/navigation/issues/634>`_)
* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
update maintainer email (lunar)
* Remove Dead Code [Lunar] (`#646 <https://github.com/ros-planning/navigation/issues/646>`_)
* Clean up navfn
* Cleanup amcl
* 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!!, Michael Ferguson, stevemacenski
1.15.1 (2017-08-14)
-------------------
1.15.0 (2017-08-07)
-------------------
* Reference Issue `#592 <https://github.com/ros-planning/navigation/issues/592>`_ Added warning to AMCL when map is published on ... (`#604 <https://github.com/ros-planning/navigation/issues/604>`_)
* rebase fixups
* convert packages to format2
* recompute cluster stat when force_publication
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
* amcl: fix compilation with gcc v7
* Added deps to amcl costmap_2d move_base (`#512 <https://github.com/ros-planning/navigation/issues/512>`_)
* fix order of parameters (closes `#553 <https://github.com/ros-planning/navigation/issues/553>`_)
* Fix potential string overflow and resource leak
* Contributors: Dmitry Rozhkov, Laurent GEORGE, Martin Günther, Michael Ferguson, Mikael Arguedas, Peter Harliman Liem, mryellow, vik748
1.14.0 (2016-05-20)
-------------------
* Allow AMCL to run from bag file to allow very fast testing.
* Fixes interpretation of a delayed initialpose message (see `#424 <https://github.com/ros-planning/navigation/issues/424>`_).
The tf lookup as it was before this change was very likely to fail as
ros::Time::now() was used to look up a tf without waiting on the tf's
availability. Additionally, the computation of the "new pose" by
multiplying the delta that the robot moved from the initialpose's
timestamp to ros::Time::now() was wrong. That delta has to by multiplied
from the right to the "old pose".
This commit also changes the reference frame to look up this delta to be
the odom frame as this one is supposed to be smooth and therefore the
best reference to get relative robot motion in the robot (base link) frame.
* New unit test for proper interpretation of a delayed initialpose message.
Modifies the set_pose.py script to be able to send an initial pose with
a user defined time stamp at a user defined time. Adds a rostest to
exercise this new option.
This reveals the issues mentioned in `#424 <https://github.com/ros-planning/navigation/issues/424>`_ (the new test fails).
* Contributors: Derek King, Stephan Wirth
1.13.1 (2015-10-29)
-------------------
* adds the set_map service to amcl
* fix pthread_mutex_lock on shutdown
* Contributors: Michael Ferguson, Stephan Wirth
1.13.0 (2015-03-17)
-------------------
* amcl_node will now save latest pose on shutdown
* Contributors: Ian Danforth
1.12.0 (2015-02-04)
-------------------
* update maintainer email
* Contributors: Michael Ferguson
1.11.15 (2015-02-03)
--------------------
1.11.14 (2014-12-05)
--------------------
1.11.13 (2014-10-02)
--------------------
1.11.12 (2014-10-01)
--------------------
* Bug fix to remove particle weights being reset when motion model is updated
* Integrated new sensor model which calculates the observation likelihood in a probabilistic manner
Also includes the option to do beam-skipping (to better handle observations from dynamic obstacles)
* Pose pulled from parameter server when new map received
* Contributors: Steven Kordell, hes3pal
1.11.11 (2014-07-23)
--------------------
1.11.10 (2014-06-25)
--------------------
1.11.9 (2014-06-10)
-------------------
1.11.8 (2014-05-21)
-------------------
1.11.7 (2014-05-21)
-------------------
* removes useless this->z_max = z_max assignment
* Fix warning string.
* Contributors: Jeremiah Via, enriquefernandez
1.11.5 (2014-01-30)
-------------------
* Fix for `#160 <https://github.com/ros-planning/navigation/issues/160>`_
* Download test data from download.ros.org instead of willow
* Change maintainer from Hersh to Lu
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* amcl_pose and particle cloud are now published latched
* Fixed or commented out failing amcl tests.

168
navigations/amcl/CMakeLists.txt Executable file
View File

@@ -0,0 +1,168 @@
cmake_minimum_required(VERSION 3.1)
project(amcl)
include(CheckIncludeFile)
include(CheckSymbolExists)
find_package(catkin REQUIRED
COMPONENTS
diagnostic_updater
dynamic_reconfigure
geometry_msgs
message_filters
nav_msgs
rosbag
roscpp
sensor_msgs
std_srvs
tf2
tf2_geometry_msgs
tf2_msgs
tf2_ros
)
find_package(Boost REQUIRED)
# dynamic reconfigure
generate_dynamic_reconfigure_options(
cfg/AMCL.cfg
)
catkin_package(
CATKIN_DEPENDS
diagnostic_updater
dynamic_reconfigure
geometry_msgs
nav_msgs
rosbag
roscpp
sensor_msgs
std_srvs
tf2
tf2_msgs
tf2_ros
INCLUDE_DIRS include
LIBRARIES amcl_sensors amcl_map amcl_pf
)
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
include_directories(src/include)
check_include_file(unistd.h HAVE_UNISTD_H)
if (HAVE_UNISTD_H)
add_definitions(-DHAVE_UNISTD_H)
endif (HAVE_UNISTD_H)
check_symbol_exists(drand48 stdlib.h HAVE_DRAND48)
if (HAVE_DRAND48)
add_definitions(-DHAVE_DRAND48)
endif (HAVE_DRAND48)
add_library(amcl_pf
src/amcl/pf/pf.c
src/amcl/pf/pf_kdtree.c
src/amcl/pf/pf_pdf.c
src/amcl/pf/pf_vector.c
src/amcl/pf/eig3.c
src/amcl/pf/pf_draw.c)
add_library(amcl_map
src/amcl/map/map.c
src/amcl/map/map_cspace.cpp
src/amcl/map/map_range.c
src/amcl/map/map_store.c
src/amcl/map/map_draw.c)
add_library(amcl_sensors
src/amcl/sensors/amcl_sensor.cpp
src/amcl/sensors/amcl_odom.cpp
src/amcl/sensors/amcl_laser.cpp)
target_link_libraries(amcl_sensors amcl_map amcl_pf)
add_executable(amcl
src/amcl_node.cpp)
add_dependencies(amcl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(amcl
amcl_sensors amcl_map amcl_pf
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
install(TARGETS
amcl
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(TARGETS
amcl_sensors amcl_map amcl_pf
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/amcl/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY examples/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples
)
## Configure Tests
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
# Bags
catkin_download_test_data(${PROJECT_NAME}_basic_localization_stage_indexed.bag
http://download.ros.org/data/amcl/basic_localization_stage_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 41fe43af189ec71e5e48eb9ed661a655)
catkin_download_test_data(${PROJECT_NAME}_global_localization_stage_indexed.bag
http://download.ros.org/data/amcl/global_localization_stage_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 752f711cf4f6e8d1d660675e2da096b0)
catkin_download_test_data(${PROJECT_NAME}_small_loop_prf_indexed.bag
http://download.ros.org/data/amcl/small_loop_prf_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 e4ef0fc006872b43f12ed8a7ce7dcd81)
catkin_download_test_data(${PROJECT_NAME}_small_loop_crazy_driving_prg_indexed.bag
http://download.ros.org/data/amcl/small_loop_crazy_driving_prg_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 4a58d1a7962914009d99000d06e5939c)
catkin_download_test_data(${PROJECT_NAME}_texas_greenroom_loop_indexed.bag
http://download.ros.org/data/amcl/texas_greenroom_loop_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 6e3432115cccdca1247f6c807038e13d)
catkin_download_test_data(${PROJECT_NAME}_texas_willow_hallway_loop_indexed.bag
http://download.ros.org/data/amcl/texas_willow_hallway_loop_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 27deb742fdcd3af44cf446f39f2688a8)
catkin_download_test_data(${PROJECT_NAME}_rosie_localization_stage.bag
http://download.ros.org/data/amcl/rosie_localization_stage.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 3347bf3835724cfa45e958c5c1846066)
# Maps
catkin_download_test_data(${PROJECT_NAME}_willow-full.pgm
http://download.ros.org/data/amcl/willow-full.pgm
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 b84465cdbbfe3e2fb9eb4579e0bcaf0e)
catkin_download_test_data(${PROJECT_NAME}_willow-full-0.05.pgm
http://download.ros.org/data/amcl/willow-full-0.05.pgm
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 b61694296e08965096c5e78611fd9765)
# Tests
add_rostest(test/set_initial_pose.xml)
add_rostest(test/set_initial_pose_delayed.xml)
add_rostest(test/basic_localization_stage.xml)
add_rostest(test/global_localization_stage.xml)
add_rostest(test/small_loop_prf.xml)
add_rostest(test/small_loop_crazy_driving_prg.xml)
add_rostest(test/texas_greenroom_loop.xml)
add_rostest(test/rosie_multilaser.xml)
add_rostest(test/texas_willow_hallway_loop.xml)
endif()

77
navigations/amcl/cfg/AMCL.cfg Executable file
View File

@@ -0,0 +1,77 @@
#!/usr/bin/env python
PACKAGE = 'amcl'
from math import pi
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, int_t, double_t, str_t, bool_t
gen = ParameterGenerator()
gen.add("min_particles", int_t, 0, "Minimum allowed number of particles.", 100, 0, 1000)
gen.add("max_particles", int_t, 0, "Mamimum allowed number of particles.", 5000, 0, 10000)
gen.add("kld_err", double_t, 0, "Maximum error between the true distribution and the estimated distribution.", .01, 0, 1)
gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distribution will be less than kld_err.", .99, 0, 1)
gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter update.", .2, 0, 5)
gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter update.", pi/6, 0, 2*pi)
gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20)
gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2)
gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.", 0, 0, .5)
gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.", 0, 0, 1)
gen.add("do_beamskip", bool_t, 0, "When true skips laser scans when a scan doesnt work for a majority of particles", False)
gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is considered invalid", 0.5, 0, 2)
gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0.3, 0, 1)
gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. When false, do not.", True)
gen.add("force_update_after_initialpose", bool_t, 0, "When true, force a pose update after a new /initialpose is received.", False)
gen.add("force_update_after_set_map", bool_t, 0, "When true, force a pose update after a new map (and pose) has been set via the /set_map service.", False)
gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100)
gen.add("save_pose_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter. -1.0 to disable.", .5, -1, 10)
gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.", False)
gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received.", False)
# Laser Model Parameters
gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used.", -1, -1, 1000)
gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used.", -1, -1, 1000)
gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when updating the filter.", 30, 0, 250)
gen.add("laser_z_hit", double_t, 0, "Mixture weight for the z_hit part of the model.", .95, 0, 1)
gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", .1, 0, 1)
gen.add("laser_z_max", double_t, 0, "Mixture weight for the z_max part of the model.", .05, 0, 1)
gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", .05, 0, 1)
gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit part of the model.", .2, 0, 10)
gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part of model.", .1, 0, 10)
gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on map, for use in likelihood_field model.", 2, 0, 20)
lmt = gen.enum([gen.const("beam_const", str_t, "beam", "Use beam laser model"), gen.const("likelihood_field_const", str_t, "likelihood_field", "Use likelihood_field laser model"), gen.const("likelihood_field_prob", str_t, "likelihood_field_prob", "Use likelihood_field_prob laser model")], "Laser Models")
gen.add("laser_model_type", str_t, 0, "Which model to use, either beam, likelihood_field or likelihood_field_prob.", "likelihood_field", edit_method=lmt)
# Odometry Model Parameters
odt = gen.enum([gen.const("diff_const", str_t, "diff", "Use diff odom model"),
gen.const("omni_const", str_t, "omni", "Use omni odom model"),
gen.const("diff_corrected_const", str_t, "diff-corrected", "Use corrected diff odom model"),
gen.const("omni_corrected_const", str_t, "omni-corrected", "Use corrected omni odom model")],
"Odom Models")
gen.add("odom_model_type", str_t, 0, "Which model to use, diff, omni, diff-corrected, or omni-corrected", "diff", edit_method=odt)
gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the translational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha5", double_t, 0, "Translation-related noise parameter (only used if model is omni).", .2, 0, 10)
gen.add("odom_frame_id", str_t, 0, "Which frame to use for odometry.", "odom")
gen.add("base_frame_id", str_t, 0, "Which frame to use for the robot base.", "base_link")
gen.add("global_frame_id", str_t, 0, "The name of the coordinate frame published by the localization system.", "map")
gen.add("restore_defaults", bool_t, 0, "Retsore the default configuration", False)
exit(gen.generate(PACKAGE, "amcl_node", "AMCL"))

34
navigations/amcl/examples/amcl_diff.launch Executable file
View File

@@ -0,0 +1,34 @@
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

34
navigations/amcl/examples/amcl_omni.launch Executable file
View File

@@ -0,0 +1,34 @@
<launch>
<node pkg="amcl" type="amcl" name="amcl">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

150
navigations/amcl/include/amcl/map/map.h Executable file
View File

@@ -0,0 +1,150 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map (grid-based)
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $
**************************************************************************/
#ifndef MAP_H
#define MAP_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
// Forward declarations
struct _rtk_fig_t;
// Limits
#define MAP_WIFI_MAX_LEVELS 8
// Description for a single map cell.
typedef struct
{
// Occupancy state (-1 = free, 0 = unknown, +1 = occ)
int occ_state;
// Distance to the nearest occupied cell
double occ_dist;
// Wifi levels
//int wifi_levels[MAP_WIFI_MAX_LEVELS];
} map_cell_t;
// Description for a map
typedef struct
{
// Map origin; the map is a viewport onto a conceptual larger map.
double origin_x, origin_y;
// Map scale (m/cell)
double scale;
// Map dimensions (number of cells)
int size_x, size_y;
// The map data, stored as a grid
map_cell_t *cells;
// Max distance at which we care about obstacles, for constructing
// likelihood field
double max_occ_dist;
} map_t;
/**************************************************************************
* Basic map functions
**************************************************************************/
// Create a new (empty) map
map_t *map_alloc(void);
// Destroy a map
void map_free(map_t *map);
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa);
// Load an occupancy map
int map_load_occ(map_t *map, const char *filename, double scale, int negate);
// Load a wifi signal strength map
//int map_load_wifi(map_t *map, const char *filename, int index);
// Update the cspace distances
void map_update_cspace(map_t *map, double max_occ_dist);
/**************************************************************************
* Range functions
**************************************************************************/
// Extract a single range reading from the map
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range);
/**************************************************************************
* GUI/diagnostic functions
**************************************************************************/
// Draw the occupancy grid
void map_draw_occ(map_t *map, struct _rtk_fig_t *fig);
// Draw the cspace map
void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig);
// Draw a wifi map
void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index);
/**************************************************************************
* Map manipulation macros
**************************************************************************/
// Convert from map index to world coords
#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)
// Convert from world coords to map coords
#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)
// Test to see if the given map coords lie within the absolute map bounds.
#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))
// Compute the cell index for the given map coords.
#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
#ifdef __cplusplus
}
#endif
#endif

11
navigations/amcl/include/amcl/pf/eig3.h Executable file
View File

@@ -0,0 +1,11 @@
/* Eigen-decomposition for symmetric 3x3 real matrices.
Public domain, copied from the public domain Java library JAMA. */
#ifndef _eig_h
/* Symmetric matrix A => eigenvectors in columns of V, corresponding
eigenvalues in d. */
void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
#endif

210
navigations/amcl/include/amcl/pf/pf.h Executable file
View File

@@ -0,0 +1,210 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Simple particle filter for localization.
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $
*************************************************************************/
#ifndef PF_H
#define PF_H
#include "pf_vector.h"
#include "pf_kdtree.h"
#ifdef __cplusplus
extern "C" {
#endif
// Forward declarations
struct _pf_t;
struct _rtk_fig_t;
struct _pf_sample_set_t;
// Function prototype for the initialization model; generates a sample pose from
// an appropriate distribution.
typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
// Function prototype for the action model; generates a sample pose from
// an appropriate distribution
typedef void (*pf_action_model_fn_t) (void *action_data,
struct _pf_sample_set_t* set);
// Function prototype for the sensor model; determines the probability
// for the given set of sample poses.
typedef double (*pf_sensor_model_fn_t) (void *sensor_data,
struct _pf_sample_set_t* set);
// Information for a single sample
typedef struct
{
// Pose represented by this sample
pf_vector_t pose;
// Weight for this pose
double weight;
} pf_sample_t;
// Information for a cluster of samples
typedef struct
{
// Number of samples
int count;
// Total weight of samples in this cluster
double weight;
// Cluster statistics
pf_vector_t mean;
pf_matrix_t cov;
// Workspace
double m[4], c[2][2];
} pf_cluster_t;
// Information for a set of samples
typedef struct _pf_sample_set_t
{
// The samples
int sample_count;
pf_sample_t *samples;
// A kdtree encoding the histogram
pf_kdtree_t *kdtree;
// Clusters
int cluster_count, cluster_max_count;
pf_cluster_t *clusters;
// Filter statistics
pf_vector_t mean;
pf_matrix_t cov;
int converged;
double n_effective;
} pf_sample_set_t;
// Information for an entire filter
typedef struct _pf_t
{
// This min and max number of samples
int min_samples, max_samples;
// Population size parameters
double pop_err, pop_z;
// Resample limit cache
int *limit_cache;
// The sample sets. We keep two sets and use [current_set]
// to identify the active set.
int current_set;
pf_sample_set_t sets[2];
// Running averages, slow and fast, of likelihood
double w_slow, w_fast;
// Decay rates for running averages
double alpha_slow, alpha_fast;
// Function used to draw random pose samples
pf_init_model_fn_t random_pose_fn;
void *random_pose_data;
double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged
int converged;
// boolean parameter to enamble/diable selective resampling
int selective_resampling;
} pf_t;
// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void *random_pose_data);
// Free an existing filter
void pf_free(pf_t *pf);
// Initialize the filter using a guassian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
// Initialize the filter using some model
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
// Resample the distribution
void pf_update_resample(pf_t *pf);
// set selective resampling parameter
void pf_set_selective_resampling(pf_t *pf, int selective_resampling);
// Compute the CEP statistics (mean and variance).
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
// Compute the statistics for a particular cluster. Returns 0 if
// there is no such cluster.
int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
pf_vector_t *mean, pf_matrix_t *cov);
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set);
// Display the sample set
void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
// Draw the histogram (kdtree)
void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
// Draw the CEP statistics
void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
// Draw the cluster statistics
void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
//calculate if the particle filter has converged -
//and sets the converged flag in the current set and the pf
int pf_update_converged(pf_t *pf);
//sets the current set and pf converged values to zero
void pf_init_converged(pf_t *pf);
void pf_copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b);
#ifdef __cplusplus
}
#endif
#endif

109
navigations/amcl/include/amcl/pf/pf_kdtree.h Executable file
View File

@@ -0,0 +1,109 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: KD tree functions
* Author: Andrew Howard
* Date: 18 Dec 2002
* CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $
*************************************************************************/
#ifndef PF_KDTREE_H
#define PF_KDTREE_H
#ifdef INCLUDE_RTKGUI
#include "rtk.h"
#endif
// Info for a node in the tree
typedef struct pf_kdtree_node
{
// Depth in the tree
int leaf, depth;
// Pivot dimension and value
int pivot_dim;
double pivot_value;
// The key for this node
int key[3];
// The value for this node
double value;
// The cluster label (leaf nodes)
int cluster;
// Child nodes
struct pf_kdtree_node *children[2];
} pf_kdtree_node_t;
// A kd tree
typedef struct
{
// Cell size
double size[3];
// The root node of the tree
pf_kdtree_node_t *root;
// The number of nodes in the tree
int node_count, node_max_count;
pf_kdtree_node_t *nodes;
// The number of leaf nodes in the tree
int leaf_count;
} pf_kdtree_t;
// Create a tree
extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
// Destroy a tree
extern void pf_kdtree_free(pf_kdtree_t *self);
// Clear all entries from the tree
extern void pf_kdtree_clear(pf_kdtree_t *self);
// Insert a pose into the tree
extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
// Cluster the leaves in the tree
extern void pf_kdtree_cluster(pf_kdtree_t *self);
// Determine the probability estimate for the given pose
extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
// Determine the cluster label for the given pose
extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
#ifdef INCLUDE_RTKGUI
// Draw the tree
extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
#endif
#endif

85
navigations/amcl/include/amcl/pf/pf_pdf.h Executable file
View File

@@ -0,0 +1,85 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Useful pdf functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#ifndef PF_PDF_H
#define PF_PDF_H
#include "pf_vector.h"
//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>
#ifdef __cplusplus
extern "C" {
#endif
/**************************************************************************
* Gaussian
*************************************************************************/
// Gaussian PDF info
typedef struct
{
// Mean, covariance and inverse covariance
pf_vector_t x;
pf_matrix_t cx;
//pf_matrix_t cxi;
double cxdet;
// Decomposed covariance matrix (rotation * diagonal)
pf_matrix_t cr;
pf_vector_t cd;
// A random number generator
//gsl_rng *rng;
} pf_pdf_gaussian_t;
// Create a gaussian pdf
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);
// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf);
// Compute the value of the pdf at some point [z].
//double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
// http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma);
// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf);
#ifdef __cplusplus
}
#endif
#endif

94
navigations/amcl/include/amcl/pf/pf_vector.h Executable file
View File

@@ -0,0 +1,94 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Vector functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#ifndef PF_VECTOR_H
#define PF_VECTOR_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
// The basic vector
typedef struct
{
double v[3];
} pf_vector_t;
// The basic matrix
typedef struct
{
double m[3][3];
} pf_matrix_t;
// Return a zero vector
pf_vector_t pf_vector_zero();
// Check for NAN or INF in any component
int pf_vector_finite(pf_vector_t a);
// Print a vector
void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt);
// Simple vector addition
pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
// Simple vector subtraction
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
// Transform from global to local coords (a - b)
pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
// Return a zero matrix
pf_matrix_t pf_matrix_zero();
// Check for NAN or INF in any component
int pf_matrix_finite(pf_matrix_t a);
// Print a matrix
void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt);
// Compute the matrix inverse. Will also return the determinant,
// which should be checked for underflow (indicated singular matrix).
//pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
// Decompose a covariance matrix [a] into a rotation matrix [r] and a
// diagonal matrix [d] such that a = r * d * r^T.
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a);
#ifdef __cplusplus
}
#endif
#endif

156
navigations/amcl/include/amcl/sensors/amcl_laser.h Executable file
View File

@@ -0,0 +1,156 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey et al.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: LASER sensor model for AMCL
// Author: Andrew Howard
// Date: 17 Aug 2003
// CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $
//
///////////////////////////////////////////////////////////////////////////
#ifndef AMCL_LASER_H
#define AMCL_LASER_H
#include "amcl_sensor.h"
#include "../map/map.h"
namespace amcl
{
typedef enum
{
LASER_MODEL_BEAM,
LASER_MODEL_LIKELIHOOD_FIELD,
LASER_MODEL_LIKELIHOOD_FIELD_PROB
} laser_model_t;
// Laser sensor data
class AMCLLaserData : public AMCLSensorData
{
public:
AMCLLaserData () {ranges=NULL;};
virtual ~AMCLLaserData() {delete [] ranges;};
// Laser range data (range, bearing tuples)
public: int range_count;
public: double range_max;
public: double (*ranges)[2];
};
// Laseretric sensor model
class AMCLLaser : public AMCLSensor
{
// Default constructor
public: AMCLLaser(size_t max_beams, map_t* map);
public: virtual ~AMCLLaser();
public: void SetModelBeam(double z_hit,
double z_short,
double z_max,
double z_rand,
double sigma_hit,
double lambda_short,
double chi_outlier);
public: void SetModelLikelihoodField(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist);
//a more probabilistically correct model - also with the option to do beam skipping
public: void SetModelLikelihoodFieldProb(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist,
bool do_beamskip,
double beam_skip_distance,
double beam_skip_threshold,
double beam_skip_error_threshold);
// Update the filter based on the sensor model. Returns true if the
// filter has been updated.
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
// Set the laser's pose after construction
public: void SetLaserPose(pf_vector_t& laser_pose)
{this->laser_pose = laser_pose;}
// Determine the probability for the given pose
private: static double BeamModel(AMCLLaserData *data,
pf_sample_set_t* set);
// Determine the probability for the given pose
private: static double LikelihoodFieldModel(AMCLLaserData *data,
pf_sample_set_t* set);
// Determine the probability for the given pose - more probablistic model
private: static double LikelihoodFieldModelProb(AMCLLaserData *data,
pf_sample_set_t* set);
private: void reallocTempData(int max_samples, int max_obs);
private: laser_model_t model_type;
// Current data timestamp
private: double time;
// The laser map
private: map_t *map;
// Laser offset relative to robot
private: pf_vector_t laser_pose;
// Max beams to consider
private: int max_beams;
// Beam skipping parameters (used by LikelihoodFieldModelProb model)
private: bool do_beamskip;
private: double beam_skip_distance;
private: double beam_skip_threshold;
//threshold for the ratio of invalid beams - at which all beams are integrated to the likelihoods
//this would be an error condition
private: double beam_skip_error_threshold;
//temp data that is kept before observations are integrated to each particle (requried for beam skipping)
private: int max_samples;
private: int max_obs;
private: double **temp_obs;
// Laser model params
//
// Mixture params for the components of the model; must sum to 1
private: double z_hit;
private: double z_short;
private: double z_max;
private: double z_rand;
//
// Stddev of Gaussian model for laser hits.
private: double sigma_hit;
// Decay rate of exponential model for short readings.
private: double lambda_short;
// Threshold for outlier rejection (unused)
private: double chi_outlier;
};
}
#endif

54
navigations/amcl/include/amcl/sensors/amcl_laser.puml Executable file
View File

@@ -0,0 +1,54 @@
@startuml
class Player {
+ Player()
}
class AMCLLaserData {
+ AMCLLaserData()
+ ~AMCLLaserData()
# range_count: int
# range_max: double
# ranges: double[][]
}
class AMCLLaser {
- model_type: laser_model_t
- time: double
- map: map_t
- laser_pose: pf_vector_t
- max_beams: int
- do_beamskip: bool
- beam_skip_distance: double
- beam_skip_threshold: double
- beam_skip_error_threshold: double
- max_samples: int
- max_obs: int
- temp_obs: double[][]
- z_hit: double
- z_short: double
- z_max: double
- z_rand: double
- sigma_hit: double
- lambda_short: double
- chi_outlier: double
+ AMCLLaser(max_beams: size_t, map: map_t)
+ ~AMCLLaser()
+ SetModelBeam(z_hit: double, z_short: double, z_max: double, z_rand: double, sigma_hit: double, lambda_short: double, chi_outlier: double)
+ SetModelLikelihoodField(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double)
+ SetModelLikelihoodFieldProb(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double, do_beamskip: bool, beam_skip_distance: double, beam_skip_threshold: double, beam_skip_error_threshold: double)
+ UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+ SetLaserPose(laser_pose: pf_vector_t)
+ BeamModel(data: AMCLLaserData, set: pf_sample_set_t): double
+ LikelihoodFieldModel(data: AMCLLaserData, set: pf_sample_set_t): double
+ LikelihoodFieldModelProb(data: AMCLLaserData, set: pf_sample_set_t): double
+ reallocTempData(max_samples: int, max_obs: int)
}
Player --|> AMCLSensor
AMCLLaserData --|> AMCLSensorData
@enduml

98
navigations/amcl/include/amcl/sensors/amcl_odom.h Executable file
View File

@@ -0,0 +1,98 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey et al.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: Odometry sensor model for AMCL
// Author: Andrew Howard
// Date: 17 Aug 2003
// CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $
//
///////////////////////////////////////////////////////////////////////////
#ifndef AMCL_ODOM_H
#define AMCL_ODOM_H
#include "amcl_sensor.h"
#include "../pf/pf_pdf.h"
namespace amcl
{
typedef enum
{
ODOM_MODEL_DIFF,
ODOM_MODEL_OMNI,
ODOM_MODEL_DIFF_CORRECTED,
ODOM_MODEL_OMNI_CORRECTED
} odom_model_t;
// Odometric sensor data
class AMCLOdomData : public AMCLSensorData
{
// Odometric pose
public: pf_vector_t pose;
// Change in odometric pose
public: pf_vector_t delta;
};
// Odometric sensor model
class AMCLOdom : public AMCLSensor
{
// Default constructor
public: AMCLOdom();
public: void SetModelDiff(double alpha1,
double alpha2,
double alpha3,
double alpha4);
public: void SetModelOmni(double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5);
public: void SetModel( odom_model_t type,
double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5 = 0 );
// Update the filter based on the action model. Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
// Current data timestamp
private: double time;
// Model type
private: odom_model_t model_type;
// Drift parameters
private: double alpha1, alpha2, alpha3, alpha4, alpha5;
};
}
#endif

54
navigations/amcl/include/amcl/sensors/amcl_odom.puml Executable file
View File

@@ -0,0 +1,54 @@
@startuml
class Player {
+ Player()
}
class AMCLSensor {
- is_action: bool
- pose: pf_vector_t
+ AMCLSensor()
+ ~AMCLSensor()
+ UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+ InitSensor(pf: pf_t, data: AMCLSensorData): bool
+ UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
}
class AMCLSensorData {
- sensor: AMCLSensor
+ AMCLSensorData()
+ ~AMCLSensorData()
}
class AMCLOdomData {
- pose: pf_vector_t
- delta: pf_vector_t
+ AMCLOdomData()
}
class AMCLOdom {
- time: double
- model_type: odom_model_t
- alpha1: double
- alpha2: double
- alpha3: double
- alpha4: double
- alpha5: double
+ AMCLOdom()
+ SetModelDiff(alpha1: double, alpha2: double, alpha3: double, alpha4: double)
+ SetModelOmni(alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double)
+ SetModel(type: odom_model_t, alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double = 0)
+ UpdateAction(pf: pf_t, data: AMCLSensorData): bool
}
Player --|> AMCLSensor
AMCLSensorData --|> AMCLSensor
AMCLOdomData --|> AMCLSensorData
AMCLOdom --|> AMCLSensor
@enduml

97
navigations/amcl/include/amcl/sensors/amcl_sensor.h Executable file
View File

@@ -0,0 +1,97 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey et al.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: Adaptive Monte-Carlo localization
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $
//
///////////////////////////////////////////////////////////////////////////
#ifndef AMCL_SENSOR_H
#define AMCL_SENSOR_H
#include "../pf/pf.h"
namespace amcl
{
// Forward declarations
class AMCLSensorData;
// Base class for all AMCL sensors
class AMCLSensor
{
// Default constructor
public: AMCLSensor();
// Default destructor
public: virtual ~AMCLSensor();
// Update the filter based on the action model. Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
// Initialize the filter based on the sensor model. Returns true if the
// filter has been initialized.
public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data);
// Update the filter based on the sensor model. Returns true if the
// filter has been updated.
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
// Flag is true if this is the action sensor
public: bool is_action;
// Action pose (action sensors only)
public: pf_vector_t pose;
// AMCL Base
//protected: AdaptiveMCL & AMCL;
#ifdef INCLUDE_RTKGUI
// Setup the GUI
public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
// Finalize the GUI
public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
// Draw sensor data
public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data);
#endif
};
// Base class for all AMCL sensor measurements
class AMCLSensorData
{
// Pointer to sensor that generated the data
public: AMCLSensor *sensor;
virtual ~AMCLSensorData() {}
// Data timestamp
public: double time;
};
}
#endif

29
navigations/amcl/include/amcl/sensors/amcl_sensor.puml Executable file
View File

@@ -0,0 +1,29 @@
@startuml
class Player {
+ Player()
}
class AMCLSensor {
- is_action: bool
- pose: pf_vector_t
+ AMCLSensor()
+ ~AMCLSensor()
+ UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+ InitSensor(pf: pf_t, data: AMCLSensorData): bool
+ UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
}
class AMCLSensorData {
- sensor: AMCLSensor
+ AMCLSensorData()
+ ~AMCLSensorData()
}
Player --|> AMCLSensor
AMCLSensorData --|> AMCLSensor
@enduml

47
navigations/amcl/package.xml Executable file
View File

@@ -0,0 +1,47 @@
<?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>amcl</name>
<version>1.17.3</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
2D. It implements the adaptive (or KLD-sampling) Monte Carlo
localization approach (as described by Dieter Fox), which uses a
particle filter to track the pose of a robot against a known map.
</p>
<p>
This node is derived, with thanks, from Andrew Howard's excellent
'amcl' Player driver.
</p>
</description>
<url>http://wiki.ros.org/amcl</url>
<author>Brian P. Gerkey</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>LGPL</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_filters</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<depend>diagnostic_updater</depend>
<depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>rosbag</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>
<test_depend>map_server</test_depend>
<test_depend>rostest</test_depend>
<test_depend>python3-pykdl</test_depend>
<test_depend>tf2_py</test_depend>
</package>

84
navigations/amcl/src/amcl/map/map.c Executable file
View File

@@ -0,0 +1,84 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map (grid-based)
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "amcl/map/map.h"
// Create a new map
map_t *map_alloc(void)
{
map_t *map;
map = (map_t*) malloc(sizeof(map_t));
// Assume we start at (0, 0)
map->origin_x = 0;
map->origin_y = 0;
// Make the size odd
map->size_x = 0;
map->size_y = 0;
map->scale = 0;
// Allocate storage for main map
map->cells = (map_cell_t*) NULL;
return map;
}
// Destroy a map
void map_free(map_t *map)
{
free(map->cells);
free(map);
return;
}
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa)
{
int i, j;
map_cell_t *cell;
i = MAP_GXWX(map, ox);
j = MAP_GYWY(map, oy);
if (!MAP_VALID(map, i, j))
return NULL;
cell = map->cells + MAP_INDEX(map, i, j);
return cell;
}

176
navigations/amcl/src/amcl/map/map_cspace.cpp Executable file
View File

@@ -0,0 +1,176 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <queue>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/map/map.h"
class CellData
{
public:
map_t* map_;
unsigned int i_, j_;
unsigned int src_i_, src_j_;
};
class CachedDistanceMap
{
public:
CachedDistanceMap(double scale, double max_dist) :
distances_(NULL), scale_(scale), max_dist_(max_dist)
{
cell_radius_ = max_dist / scale;
distances_ = new double *[cell_radius_+2];
for(int i=0; i<=cell_radius_+1; i++)
{
distances_[i] = new double[cell_radius_+2];
for(int j=0; j<=cell_radius_+1; j++)
{
distances_[i][j] = sqrt(i*i + j*j);
}
}
}
~CachedDistanceMap()
{
if(distances_)
{
for(int i=0; i<=cell_radius_+1; i++)
delete[] distances_[i];
delete[] distances_;
}
}
double** distances_;
double scale_;
double max_dist_;
int cell_radius_;
};
bool operator<(const CellData& a, const CellData& b)
{
return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
}
CachedDistanceMap*
get_distance_map(double scale, double max_dist)
{
static CachedDistanceMap* cdm = NULL;
if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
{
if(cdm)
delete cdm;
cdm = new CachedDistanceMap(scale, max_dist);
}
return cdm;
}
void enqueue(map_t* map, int i, int j,
int src_i, int src_j,
std::priority_queue<CellData>& Q,
CachedDistanceMap* cdm,
unsigned char* marked)
{
if(marked[MAP_INDEX(map, i, j)])
return;
int di = abs(i - src_i);
int dj = abs(j - src_j);
double distance = cdm->distances_[di][dj];
if(distance > cdm->cell_radius_)
return;
map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
CellData cell;
cell.map_ = map;
cell.i_ = i;
cell.j_ = j;
cell.src_i_ = src_i;
cell.src_j_ = src_j;
Q.push(cell);
marked[MAP_INDEX(map, i, j)] = 1;
}
// Update the cspace distance values
void map_update_cspace(map_t *map, double max_occ_dist)
{
unsigned char* marked;
std::priority_queue<CellData> Q;
marked = new unsigned char[map->size_x*map->size_y];
memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
map->max_occ_dist = max_occ_dist;
CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
// Enqueue all the obstacle cells
CellData cell;
cell.map_ = map;
for(int i=0; i<map->size_x; i++)
{
cell.src_i_ = cell.i_ = i;
for(int j=0; j<map->size_y; j++)
{
if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
{
map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
cell.src_j_ = cell.j_ = j;
marked[MAP_INDEX(map, i, j)] = 1;
Q.push(cell);
}
else
map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
}
}
while(!Q.empty())
{
CellData current_cell = Q.top();
if(current_cell.i_ > 0)
enqueue(map, current_cell.i_-1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if(current_cell.j_ > 0)
enqueue(map, current_cell.i_, current_cell.j_-1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.i_ < map->size_x - 1)
enqueue(map, current_cell.i_+1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.j_ < map->size_y - 1)
enqueue(map, current_cell.i_, current_cell.j_+1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
Q.pop();
}
delete[] marked;
}

158
navigations/amcl/src/amcl/map/map_draw.c Executable file
View File

@@ -0,0 +1,158 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Local map GUI functions
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
**************************************************************************/
#ifdef INCLUDE_RTKGUI
#include <errno.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <rtk.h>
#include "amcl/map/map.h"
////////////////////////////////////////////////////////////////////////////
// Draw the occupancy map
void map_draw_occ(map_t *map, rtk_fig_t *fig)
{
int i, j;
int col;
map_cell_t *cell;
uint16_t *image;
uint16_t *pixel;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
// Draw occupancy
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
pixel = image + (j * map->size_x + i);
col = 127 - 127 * cell->occ_state;
*pixel = RTK_RGB16(col, col, col);
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL);
free(image);
return;
}
////////////////////////////////////////////////////////////////////////////
// Draw the cspace map
void map_draw_cspace(map_t *map, rtk_fig_t *fig)
{
int i, j;
int col;
map_cell_t *cell;
uint16_t *image;
uint16_t *pixel;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
// Draw occupancy
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
pixel = image + (j * map->size_x + i);
col = 255 * cell->occ_dist / map->max_occ_dist;
*pixel = RTK_RGB16(col, col, col);
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL);
free(image);
return;
}
////////////////////////////////////////////////////////////////////////////
// Draw a wifi map
void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index)
{
int i, j;
int level, col;
map_cell_t *cell;
uint16_t *image, *mask;
uint16_t *ipix, *mpix;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
// Draw wifi levels
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
ipix = image + (j * map->size_x + i);
mpix = mask + (j * map->size_x + i);
level = cell->wifi_levels[index];
if (cell->occ_state == -1 && level != 0)
{
col = 255 * (100 + level) / 100;
*ipix = RTK_RGB16(col, col, col);
*mpix = 1;
}
else
{
*mpix = 0;
}
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, mask);
free(mask);
free(image);
return;
}
#endif

120
navigations/amcl/src/amcl/map/map_range.c Executable file
View File

@@ -0,0 +1,120 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Range routines
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "amcl/map/map.h"
// Extract a single range reading from the map. Unknown cells and/or
// out-of-bound cells are treated as occupied, which makes it easy to
// use Stage bitmap files.
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
{
// Bresenham raytracing
int x0,x1,y0,y1;
int x,y;
int xstep, ystep;
char steep;
int tmp;
int deltax, deltay, error, deltaerr;
x0 = MAP_GXWX(map,ox);
y0 = MAP_GYWY(map,oy);
x1 = MAP_GXWX(map,ox + max_range * cos(oa));
y1 = MAP_GYWY(map,oy + max_range * sin(oa));
if(abs(y1-y0) > abs(x1-x0))
steep = 1;
else
steep = 0;
if(steep)
{
tmp = x0;
x0 = y0;
y0 = tmp;
tmp = x1;
x1 = y1;
y1 = tmp;
}
deltax = abs(x1-x0);
deltay = abs(y1-y0);
error = 0;
deltaerr = deltay;
x = x0;
y = y0;
if(x0 < x1)
xstep = 1;
else
xstep = -1;
if(y0 < y1)
ystep = 1;
else
ystep = -1;
if(steep)
{
if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
else
{
if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
while(x != (x1 + xstep * 1))
{
x += xstep;
error += deltaerr;
if(2*error >= deltax)
{
y += ystep;
error -= deltax;
}
if(steep)
{
if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
else
{
if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
}
return max_range;
}

215
navigations/amcl/src/amcl/map/map_store.c Executable file
View File

@@ -0,0 +1,215 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map storage functions
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map_store.c 2951 2005-08-19 00:48:20Z gerkey $
**************************************************************************/
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/map/map.h"
////////////////////////////////////////////////////////////////////////////
// Load an occupancy grid
int map_load_occ(map_t *map, const char *filename, double scale, int negate)
{
FILE *file;
char magic[3];
int i, j;
int ch, occ;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
fclose(file);
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
{
fprintf(stderr, "Failed ot read image dimensions");
return -1;
}
// Allocate space in the map
if (map->cells == NULL)
{
map->scale = scale;
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
//PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
// Black-on-white images
if (!negate)
{
if (ch < depth / 4)
occ = +1;
else if (ch > 3 * depth / 4)
occ = -1;
else
occ = 0;
}
// White-on-black images
else
{
if (ch < depth / 4)
occ = -1;
else if (ch > 3 * depth / 4)
occ = +1;
else
occ = 0;
}
if (!MAP_VALID(map, i, j))
continue;
cell = map->cells + MAP_INDEX(map, i, j);
cell->occ_state = occ;
}
}
fclose(file);
return 0;
}
////////////////////////////////////////////////////////////////////////////
// Load a wifi signal strength map
/*
int map_load_wifi(map_t *map, const char *filename, int index)
{
FILE *file;
char magic[3];
int i, j;
int ch, level;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
fscanf(file, "%10s \n", magic);
if (strcmp(magic, "P5") != 0)
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
fscanf(file, " %d %d \n %d \n", &width, &height, &depth);
// Allocate space in the map
if (map->cells == NULL)
{
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
//PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
if (!MAP_VALID(map, i, j))
continue;
if (ch == 0)
level = 0;
else
level = ch * 100 / 255 - 100;
cell = map->cells + MAP_INDEX(map, i, j);
cell->wifi_levels[index] = level;
}
}
fclose(file);
return 0;
}
*/

274
navigations/amcl/src/amcl/pf/eig3.c Executable file
View File

@@ -0,0 +1,274 @@
/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
domain Java Matrix library JAMA. */
#include <math.h>
#ifndef MAX
#define MAX(a, b) ((a)>(b)?(a):(b))
#endif
#ifdef _MSC_VER
#define n 3
#else
static int n = 3;
#endif
static double hypot2(double x, double y) {
return sqrt(x*x+y*y);
}
// Symmetric Householder reduction to tridiagonal form.
static void tred2(double V[n][n], double d[n], double e[n]) {
// This is derived from the Algol procedures tred2 by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i,j,k;
double f,g,h,hh;
for (j = 0; j < n; j++) {
d[j] = V[n-1][j];
}
// Householder reduction to tridiagonal form.
for (i = n-1; i > 0; i--) {
// Scale to avoid under/overflow.
double scale = 0.0;
double h = 0.0;
for (k = 0; k < i; k++) {
scale = scale + fabs(d[k]);
}
if (scale == 0.0) {
e[i] = d[i-1];
for (j = 0; j < i; j++) {
d[j] = V[i-1][j];
V[i][j] = 0.0;
V[j][i] = 0.0;
}
} else {
// Generate Householder vector.
for (k = 0; k < i; k++) {
d[k] /= scale;
h += d[k] * d[k];
}
f = d[i-1];
g = sqrt(h);
if (f > 0) {
g = -g;
}
e[i] = scale * g;
h = h - f * g;
d[i-1] = f - g;
for (j = 0; j < i; j++) {
e[j] = 0.0;
}
// Apply similarity transformation to remaining columns.
for (j = 0; j < i; j++) {
f = d[j];
V[j][i] = f;
g = e[j] + V[j][j] * f;
for (k = j+1; k <= i-1; k++) {
g += V[k][j] * d[k];
e[k] += V[k][j] * f;
}
e[j] = g;
}
f = 0.0;
for (j = 0; j < i; j++) {
e[j] /= h;
f += e[j] * d[j];
}
hh = f / (h + h);
for (j = 0; j < i; j++) {
e[j] -= hh * d[j];
}
for (j = 0; j < i; j++) {
f = d[j];
g = e[j];
for (k = j; k <= i-1; k++) {
V[k][j] -= (f * e[k] + g * d[k]);
}
d[j] = V[i-1][j];
V[i][j] = 0.0;
}
}
d[i] = h;
}
// Accumulate transformations.
for (i = 0; i < n-1; i++) {
V[n-1][i] = V[i][i];
V[i][i] = 1.0;
h = d[i+1];
if (h != 0.0) {
for (k = 0; k <= i; k++) {
d[k] = V[k][i+1] / h;
}
for (j = 0; j <= i; j++) {
g = 0.0;
for (k = 0; k <= i; k++) {
g += V[k][i+1] * V[k][j];
}
for (k = 0; k <= i; k++) {
V[k][j] -= g * d[k];
}
}
}
for (k = 0; k <= i; k++) {
V[k][i+1] = 0.0;
}
}
for (j = 0; j < n; j++) {
d[j] = V[n-1][j];
V[n-1][j] = 0.0;
}
V[n-1][n-1] = 1.0;
e[0] = 0.0;
}
// Symmetric tridiagonal QL algorithm.
static void tql2(double V[n][n], double d[n], double e[n]) {
// This is derived from the Algol procedures tql2, by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i,j,m,l,k;
double g,p,r,dl1,h,f,tst1,eps;
double c,c2,c3,el1,s,s2;
for (i = 1; i < n; i++) {
e[i-1] = e[i];
}
e[n-1] = 0.0;
f = 0.0;
tst1 = 0.0;
eps = pow(2.0,-52.0);
for (l = 0; l < n; l++) {
// Find small subdiagonal element
tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
m = l;
while (m < n) {
if (fabs(e[m]) <= eps*tst1) {
break;
}
m++;
}
// If m == l, d[l] is an eigenvalue,
// otherwise, iterate.
if (m > l) {
int iter = 0;
do {
iter = iter + 1; // (Could check iteration count here.)
// Compute implicit shift
g = d[l];
p = (d[l+1] - g) / (2.0 * e[l]);
r = hypot2(p,1.0);
if (p < 0) {
r = -r;
}
d[l] = e[l] / (p + r);
d[l+1] = e[l] * (p + r);
dl1 = d[l+1];
h = g - d[l];
for (i = l+2; i < n; i++) {
d[i] -= h;
}
f = f + h;
// Implicit QL transformation.
p = d[m];
c = 1.0;
c2 = c;
c3 = c;
el1 = e[l+1];
s = 0.0;
s2 = 0.0;
for (i = m-1; i >= l; i--) {
c3 = c2;
c2 = c;
s2 = s;
g = c * e[i];
h = c * p;
r = hypot2(p,e[i]);
e[i+1] = s * r;
s = e[i] / r;
c = p / r;
p = c * d[i] - s * g;
d[i+1] = h + s * (c * g + s * d[i]);
// Accumulate transformation.
for (k = 0; k < n; k++) {
h = V[k][i+1];
V[k][i+1] = s * V[k][i] + c * h;
V[k][i] = c * V[k][i] - s * h;
}
}
p = -s * s2 * c3 * el1 * e[l] / dl1;
e[l] = s * p;
d[l] = c * p;
// Check for convergence.
} while (fabs(e[l]) > eps*tst1);
}
d[l] = d[l] + f;
e[l] = 0.0;
}
// Sort eigenvalues and corresponding vectors.
for (i = 0; i < n-1; i++) {
k = i;
p = d[i];
for (j = i+1; j < n; j++) {
if (d[j] < p) {
k = j;
p = d[j];
}
}
if (k != i) {
d[k] = d[i];
d[i] = p;
for (j = 0; j < n; j++) {
p = V[j][i];
V[j][i] = V[j][k];
V[j][k] = p;
}
}
}
}
void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
int i,j;
double e[n];
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
V[i][j] = A[i][j];
}
}
tred2(V, d, e);
tql2(V, d, e);
}

763
navigations/amcl/src/amcl/pf/pf.c Executable file
View File

@@ -0,0 +1,763 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Simple particle filter for localization.
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include "amcl/pf/pf.h"
#include "amcl/pf/pf_pdf.h"
#include "amcl/pf/pf_kdtree.h"
#include "portable_utils.hpp"
// Compute the required number of samples, given that there are k bins
// with samples in them.
static int pf_resample_limit(pf_t *pf, int k);
// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
int i, j;
pf_t *pf;
pf_sample_set_t *set;
pf_sample_t *sample;
srand48(time(NULL));
pf = calloc(1, sizeof(pf_t));
pf->random_pose_fn = random_pose_fn;
pf->random_pose_data = random_pose_data;
pf->min_samples = min_samples;
pf->max_samples = max_samples;
// Control parameters for the population size calculation. [err] is
// the max error between the true distribution and the estimated
// distribution. [z] is the upper standard normal quantile for (1 -
// p), where p is the probability that the error on the estimated
// distrubition will be less than [err].
pf->pop_err = 0.01;
pf->pop_z = 3;
pf->dist_threshold = 0.5;
// Number of leaf nodes is never higher than the max number of samples
pf->limit_cache = calloc(max_samples, sizeof(int));
pf->current_set = 0;
for (j = 0; j < 2; j++)
{
set = pf->sets + j;
set->sample_count = max_samples;
set->samples = calloc(max_samples, sizeof(pf_sample_t));
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->pose.v[0] = 0.0;
sample->pose.v[1] = 0.0;
sample->pose.v[2] = 0.0;
sample->weight = 1.0 / max_samples;
}
// HACK: is 3 times max_samples enough?
set->kdtree = pf_kdtree_alloc(3 * max_samples);
set->cluster_count = 0;
set->cluster_max_count = max_samples;
set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
}
pf->w_slow = 0.0;
pf->w_fast = 0.0;
pf->alpha_slow = alpha_slow;
pf->alpha_fast = alpha_fast;
//set converged to 0
pf_init_converged(pf);
return pf;
}
// Free an existing filter
void pf_free(pf_t *pf)
{
int i;
free(pf->limit_cache);
for (i = 0; i < 2; i++)
{
free(pf->sets[i].clusters);
pf_kdtree_free(pf->sets[i].kdtree);
free(pf->sets[i].samples);
}
free(pf);
return;
}
// Initialize the filter using a gaussian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
pf_pdf_gaussian_t *pdf;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set->kdtree);
set->sample_count = pf->max_samples;
pdf = pf_pdf_gaussian_alloc(mean, cov);
// Compute the new sample poses
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = pf_pdf_gaussian_sample(pdf);
// Add sample to histogram
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
pf->w_slow = pf->w_fast = 0.0;
pf_pdf_gaussian_free(pdf);
// Re-compute cluster statistics
pf_cluster_stats(pf, set);
//set converged to 0
pf_init_converged(pf);
return;
}
// Initialize the filter using some model
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set->kdtree);
set->sample_count = pf->max_samples;
// Compute the new sample poses
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = (*init_fn) (init_data);
// Add sample to histogram
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
pf->w_slow = pf->w_fast = 0.0;
// Re-compute cluster statistics
pf_cluster_stats(pf, set);
//set converged to 0
pf_init_converged(pf);
return;
}
void pf_init_converged(pf_t *pf){
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
set->converged = 0;
pf->converged = 0;
}
int pf_update_converged(pf_t *pf)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
double total;
set = pf->sets + pf->current_set;
double mean_x = 0, mean_y = 0;
for (i = 0; i < set->sample_count; i++){
sample = set->samples + i;
mean_x += sample->pose.v[0];
mean_y += sample->pose.v[1];
}
mean_x /= set->sample_count;
mean_y /= set->sample_count;
for (i = 0; i < set->sample_count; i++){
sample = set->samples + i;
if(fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold ||
fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold){
set->converged = 0;
pf->converged = 0;
return 0;
}
}
set->converged = 1;
pf->converged = 1;
return 1;
}
// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
{
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
(*action_fn) (action_data, set);
return;
}
#include <float.h>
// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
double total;
set = pf->sets + pf->current_set;
// Compute the sample weights
total = (*sensor_fn) (sensor_data, set);
set->n_effective = 0;
if (total > 0.0)
{
// Normalize weights
double w_avg=0.0;
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
w_avg += sample->weight;
sample->weight /= total;
set->n_effective += sample->weight*sample->weight;
}
// Update running averages of likelihood of samples (Prob Rob p258)
w_avg /= set->sample_count;
if(pf->w_slow == 0.0)
pf->w_slow = w_avg;
else
pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
if(pf->w_fast == 0.0)
pf->w_fast = w_avg;
else
pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
//printf("w_avg: %e slow: %e fast: %e\n",
//w_avg, pf->w_slow, pf->w_fast);
}
else
{
// Handle zero total
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / set->sample_count;
}
}
set->n_effective = 1.0/set->n_effective;
return;
}
// copy set a to set b
void copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b)
{
int i;
double total;
pf_sample_t *sample_a, *sample_b;
// Clean set b's kdtree
pf_kdtree_clear(set_b->kdtree);
// Copy samples from set a to create set b
total = 0;
set_b->sample_count = 0;
for(i = 0; i < set_a->sample_count; i++)
{
sample_b = set_b->samples + set_b->sample_count++;
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Copy sample a to sample b
sample_b->pose = sample_a->pose;
sample_b->weight = sample_a->weight;
total += sample_b->weight;
// Add sample to histogram
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
}
// Normalize weights
for (i = 0; i < set_b->sample_count; i++)
{
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
set_b->converged = set_a->converged;
}
// Resample the distribution
void pf_update_resample(pf_t *pf)
{
int i;
double total;
pf_sample_set_t *set_a, *set_b;
pf_sample_t *sample_a, *sample_b;
//double r,c,U;
//int m;
//double count_inv;
double* c;
double w_diff;
set_a = pf->sets + pf->current_set;
set_b = pf->sets + (pf->current_set + 1) % 2;
if (pf->selective_resampling != 0)
{
if (set_a->n_effective > 0.5*(set_a->sample_count))
{
// copy set a to b
copy_set(set_a,set_b);
// Re-compute cluster statistics
pf_cluster_stats(pf, set_b);
// Use the newly created sample set
pf->current_set = (pf->current_set + 1) % 2;
return;
}
}
// Build up cumulative probability table for resampling.
// TODO: Replace this with a more efficient procedure
// (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
c[0] = 0.0;
for(i=0;i<set_a->sample_count;i++)
c[i+1] = c[i]+set_a->samples[i].weight;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set_b->kdtree);
// Draw samples from set a to create set b.
total = 0;
set_b->sample_count = 0;
w_diff = 1.0 - pf->w_fast / pf->w_slow;
if(w_diff < 0.0)
w_diff = 0.0;
//printf("w_diff: %9.6f\n", w_diff);
// Can't (easily) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// Low-variance resampler, taken from Probabilistic Robotics, p110
count_inv = 1.0/set_a->sample_count;
r = drand48() * count_inv;
c = set_a->samples[0].weight;
i = 0;
m = 0;
*/
while(set_b->sample_count < pf->max_samples)
{
sample_b = set_b->samples + set_b->sample_count++;
if(drand48() < w_diff)
sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
else
{
// Can't (easily) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// Low-variance resampler, taken from Probabilistic Robotics, p110
U = r + m * count_inv;
while(U>c)
{
i++;
// Handle wrap-around by resetting counters and picking a new random
// number
if(i >= set_a->sample_count)
{
r = drand48() * count_inv;
c = set_a->samples[0].weight;
i = 0;
m = 0;
U = r + m * count_inv;
continue;
}
c += set_a->samples[i].weight;
}
m++;
*/
// Naive discrete event sampler
double r;
r = drand48();
for(i=0;i<set_a->sample_count;i++)
{
if((c[i] <= r) && (r < c[i+1]))
break;
}
assert(i<set_a->sample_count);
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Add sample to list
sample_b->pose = sample_a->pose;
}
sample_b->weight = 1.0;
total += sample_b->weight;
// Add sample to histogram
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
// See if we have enough samples yet
if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
break;
}
// Reset averages, to avoid spiraling off into complete randomness.
if(w_diff > 0.0)
pf->w_slow = pf->w_fast = 0.0;
//fprintf(stderr, "\n\n");
// Normalize weights
for (i = 0; i < set_b->sample_count; i++)
{
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
// Re-compute cluster statistics
pf_cluster_stats(pf, set_b);
// Use the newly created sample set
pf->current_set = (pf->current_set + 1) % 2;
pf_update_converged(pf);
free(c);
return;
}
// Compute the required number of samples, given that there are k bins
// with samples in them. This is taken directly from Fox et al.
int pf_resample_limit(pf_t *pf, int k)
{
double a, b, c, x;
int n;
// Return max_samples in case k is outside expected range, this shouldn't
// happen, but is added to prevent any runtime errors
if (k < 1 || k > pf->max_samples)
return pf->max_samples;
// Return value if cache is valid, which means value is non-zero positive
if (pf->limit_cache[k-1] > 0)
return pf->limit_cache[k-1];
if (k == 1)
{
pf->limit_cache[k-1] = pf->max_samples;
return pf->max_samples;
}
a = 1;
b = 2 / (9 * ((double) k - 1));
c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
x = a - b + c;
n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
if (n < pf->min_samples)
{
pf->limit_cache[k-1] = pf->min_samples;
return pf->min_samples;
}
if (n > pf->max_samples)
{
pf->limit_cache[k-1] = pf->max_samples;
return pf->max_samples;
}
pf->limit_cache[k-1] = n;
return n;
}
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{
int i, j, k, cidx;
pf_sample_t *sample;
pf_cluster_t *cluster;
// Workspace
double m[4], c[2][2];
size_t count;
double weight;
// Cluster the samples
pf_kdtree_cluster(set->kdtree);
// Initialize cluster stats
set->cluster_count = 0;
for (i = 0; i < set->cluster_max_count; i++)
{
cluster = set->clusters + i;
cluster->count = 0;
cluster->weight = 0;
cluster->mean = pf_vector_zero();
cluster->cov = pf_matrix_zero();
for (j = 0; j < 4; j++)
cluster->m[j] = 0.0;
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
cluster->c[j][k] = 0.0;
}
// Initialize overall filter stats
count = 0;
weight = 0.0;
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
for (j = 0; j < 4; j++)
m[j] = 0.0;
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
c[j][k] = 0.0;
// Compute cluster stats
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
//printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
// Get the cluster label for this sample
cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
assert(cidx >= 0);
if (cidx >= set->cluster_max_count)
continue;
if (cidx + 1 > set->cluster_count)
set->cluster_count = cidx + 1;
cluster = set->clusters + cidx;
cluster->count += 1;
cluster->weight += sample->weight;
count += 1;
weight += sample->weight;
// Compute mean
cluster->m[0] += sample->weight * sample->pose.v[0];
cluster->m[1] += sample->weight * sample->pose.v[1];
cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
m[0] += sample->weight * sample->pose.v[0];
m[1] += sample->weight * sample->pose.v[1];
m[2] += sample->weight * cos(sample->pose.v[2]);
m[3] += sample->weight * sin(sample->pose.v[2]);
// Compute covariance in linear components
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
{
cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
}
}
// Normalize
for (i = 0; i < set->cluster_count; i++)
{
cluster = set->clusters + i;
cluster->mean.v[0] = cluster->m[0] / cluster->weight;
cluster->mean.v[1] = cluster->m[1] / cluster->weight;
cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
cluster->cov = pf_matrix_zero();
// Covariance in linear components
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
cluster->mean.v[j] * cluster->mean.v[k];
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
cluster->m[3] * cluster->m[3]) / cluster->weight);
//printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
//cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
//pf_matrix_fprintf(cluster->cov, stdout, "%e");
}
assert(fabs(weight) >= DBL_EPSILON);
if (fabs(weight) < DBL_EPSILON)
{
printf("ERROR : divide-by-zero exception : weight is zero\n");
return;
}
// Compute overall filter stats
set->mean.v[0] = m[0] / weight;
set->mean.v[1] = m[1] / weight;
set->mean.v[2] = atan2(m[3], m[2]);
// Covariance in linear components
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
return;
}
void pf_set_selective_resampling(pf_t *pf, int selective_resampling)
{
pf->selective_resampling = selective_resampling;
}
// Compute the CEP statistics (mean and variance).
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
{
int i;
double mn, mx, my, mrr;
pf_sample_set_t *set;
pf_sample_t *sample;
set = pf->sets + pf->current_set;
mn = 0.0;
mx = 0.0;
my = 0.0;
mrr = 0.0;
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
mn += sample->weight;
mx += sample->weight * sample->pose.v[0];
my += sample->weight * sample->pose.v[1];
mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
}
assert(fabs(mn) >= DBL_EPSILON);
if (fabs(mn) < DBL_EPSILON)
{
printf("ERROR : divide-by-zero exception : mn is zero\n");
return;
}
mean->v[0] = mx / mn;
mean->v[1] = my / mn;
mean->v[2] = 0.0;
*var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
return;
}
// Get the statistics for a particular cluster.
int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight,
pf_vector_t *mean, pf_matrix_t *cov)
{
pf_sample_set_t *set;
pf_cluster_t *cluster;
set = pf->sets + pf->current_set;
if (clabel >= set->cluster_count)
return 0;
cluster = set->clusters + clabel;
*weight = cluster->weight;
*mean = cluster->mean;
*cov = cluster->cov;
return 1;
}

163
navigations/amcl/src/amcl/pf/pf_draw.c Executable file
View File

@@ -0,0 +1,163 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Particle filter; drawing routines
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#ifdef INCLUDE_RTKGUI
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include "rtk.h"
#include "pf.h"
#include "pf_pdf.h"
#include "pf_kdtree.h"
// Draw the statistics
void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig);
// Draw the sample set
void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples)
{
int i;
double px, py, pa;
pf_sample_set_t *set;
pf_sample_t *sample;
set = pf->sets + pf->current_set;
max_samples = MIN(max_samples, set->sample_count);
for (i = 0; i < max_samples; i++)
{
sample = set->samples + i;
px = sample->pose.v[0];
py = sample->pose.v[1];
pa = sample->pose.v[2];
//printf("%f %f\n", px, py);
rtk_fig_point(fig, px, py);
rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
//rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
}
return;
}
// Draw the hitogram (kd tree)
void pf_draw_hist(pf_t *pf, rtk_fig_t *fig)
{
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
rtk_fig_color(fig, 0.0, 0.0, 1.0);
pf_kdtree_draw(set->kdtree, fig);
return;
}
// Draw the CEP statistics
void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig)
{
pf_vector_t mean;
double var;
pf_get_cep_stats(pf, &mean, &var);
var = sqrt(var);
rtk_fig_color(fig, 0, 0, 1);
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
return;
}
// Draw the cluster statistics
void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
{
int i;
pf_cluster_t *cluster;
pf_sample_set_t *set;
pf_vector_t mean;
pf_matrix_t cov;
pf_matrix_t r, d;
double weight, o, d1, d2;
set = pf->sets + pf->current_set;
for (i = 0; i < set->cluster_count; i++)
{
cluster = set->clusters + i;
weight = cluster->weight;
mean = cluster->mean;
cov = cluster->cov;
// Compute unitary representation S = R D R^T
pf_matrix_unitary(&r, &d, cov);
/* Debugging
printf("mean = \n");
pf_vector_fprintf(mean, stdout, "%e");
printf("cov = \n");
pf_matrix_fprintf(cov, stdout, "%e");
printf("r = \n");
pf_matrix_fprintf(r, stdout, "%e");
printf("d = \n");
pf_matrix_fprintf(d, stdout, "%e");
*/
// Compute the orientation of the error ellipse (first eigenvector)
o = atan2(r.m[1][0], r.m[0][0]);
d1 = 6 * sqrt(d.m[0][0]);
d2 = 6 * sqrt(d.m[1][1]);
if (d1 > 1e-3 && d2 > 1e-3)
{
// Draw the error ellipse
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
}
// Draw a direction indicator
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
}
return;
}
#endif

486
navigations/amcl/src/amcl/pf/pf_kdtree.c Executable file
View File

@@ -0,0 +1,486 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: kd-tree functions
* Author: Andrew Howard
* Date: 18 Dec 2002
* CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/pf/pf_vector.h"
#include "amcl/pf/pf_kdtree.h"
// Compare keys to see if they are equal
static int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]);
// Insert a node into the tree
static pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent,
pf_kdtree_node_t *node, int key[], double value);
// Recursive node search
static pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]);
// Recursively label nodes in this cluster
static void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth);
// Recursive node printing
//static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node);
#ifdef INCLUDE_RTKGUI
// Recursively draw nodes
static void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig);
#endif
////////////////////////////////////////////////////////////////////////////////
// Create a tree
pf_kdtree_t *pf_kdtree_alloc(int max_size)
{
pf_kdtree_t *self;
self = calloc(1, sizeof(pf_kdtree_t));
self->size[0] = 0.50;
self->size[1] = 0.50;
self->size[2] = (10 * M_PI / 180);
self->root = NULL;
self->node_count = 0;
self->node_max_count = max_size;
self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t));
self->leaf_count = 0;
return self;
}
////////////////////////////////////////////////////////////////////////////////
// Destroy a tree
void pf_kdtree_free(pf_kdtree_t *self)
{
free(self->nodes);
free(self);
return;
}
////////////////////////////////////////////////////////////////////////////////
// Clear all entries from the tree
void pf_kdtree_clear(pf_kdtree_t *self)
{
self->root = NULL;
self->leaf_count = 0;
self->node_count = 0;
return;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a pose into the tree.
void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
{
int key[3];
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);
// Test code
/*
printf("find %d %d %d\n", key[0], key[1], key[2]);
assert(pf_kdtree_find_node(self, self->root, key) != NULL);
pf_kdtree_print_node(self, self->root);
printf("\n");
for (i = 0; i < self->node_count; i++)
{
node = self->nodes + i;
if (node->leaf)
{
printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]);
assert(pf_kdtree_find_node(self, self->root, node->key) == node);
}
}
printf("\n\n");
*/
return;
}
////////////////////////////////////////////////////////////////////////////////
// Determine the probability estimate for the given pose. TODO: this
// should do a kernel density estimate rather than a simple histogram.
double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose)
{
int key[3];
pf_kdtree_node_t *node;
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
node = pf_kdtree_find_node(self, self->root, key);
if (node == NULL)
return 0.0;
return node->value;
}
////////////////////////////////////////////////////////////////////////////////
// Determine the cluster label for the given pose
int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
{
int key[3];
pf_kdtree_node_t *node;
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
node = pf_kdtree_find_node(self, self->root, key);
if (node == NULL)
return -1;
return node->cluster;
}
////////////////////////////////////////////////////////////////////////////////
// Compare keys to see if they are equal
int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[])
{
//double a, b;
if (key_a[0] != key_b[0])
return 0;
if (key_a[1] != key_b[1])
return 0;
if (key_a[2] != key_b[2])
return 0;
/* TODO: make this work (pivot selection needs fixing, too)
// Normalize angles
a = key_a[2] * self->size[2];
a = atan2(sin(a), cos(a)) / self->size[2];
b = key_b[2] * self->size[2];
b = atan2(sin(b), cos(b)) / self->size[2];
if ((int) a != (int) b)
return 0;
*/
return 1;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a node into the tree
pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent,
pf_kdtree_node_t *node, int key[], double value)
{
int i;
int split, max_split;
// If the node doesnt exist yet...
if (node == NULL)
{
assert(self->node_count < self->node_max_count);
node = self->nodes + self->node_count++;
memset(node, 0, sizeof(pf_kdtree_node_t));
node->leaf = 1;
if (parent == NULL)
node->depth = 0;
else
node->depth = parent->depth + 1;
for (i = 0; i < 3; i++)
node->key[i] = key[i];
node->value = value;
self->leaf_count += 1;
}
// If the node exists, and it is a leaf node...
else if (node->leaf)
{
// If the keys are equal, increment the value
if (pf_kdtree_equal(self, key, node->key))
{
node->value += value;
}
// The keys are not equal, so split this node
else
{
// Find the dimension with the largest variance and do a mean
// split
max_split = 0;
node->pivot_dim = -1;
for (i = 0; i < 3; i++)
{
split = abs(key[i] - node->key[i]);
if (split > max_split)
{
max_split = split;
node->pivot_dim = i;
}
}
assert(node->pivot_dim >= 0);
node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0;
if (key[node->pivot_dim] < node->pivot_value)
{
node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value);
node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
}
else
{
node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value);
}
node->leaf = 0;
self->leaf_count -= 1;
}
}
// If the node exists, and it has children...
else
{
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
if (key[node->pivot_dim] < node->pivot_value)
pf_kdtree_insert_node(self, node, node->children[0], key, value);
else
pf_kdtree_insert_node(self, node, node->children[1], key, value);
}
return node;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node search
pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[])
{
if (node->leaf)
{
//printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]);
// If the keys are the same...
if (pf_kdtree_equal(self, key, node->key))
return node;
else
return NULL;
}
else
{
//printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value);
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
// If the keys are different...
if (key[node->pivot_dim] < node->pivot_value)
return pf_kdtree_find_node(self, node->children[0], key);
else
return pf_kdtree_find_node(self, node->children[1], key);
}
return NULL;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node printing
/*
void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node)
{
if (node->leaf)
{
printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]);
printf("%*s", node->depth * 11, "");
}
else
{
printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]);
pf_kdtree_print_node(self, node->children[0]);
pf_kdtree_print_node(self, node->children[1]);
}
return;
}
*/
////////////////////////////////////////////////////////////////////////////////
// Cluster the leaves in the tree
void pf_kdtree_cluster(pf_kdtree_t *self)
{
int i;
int queue_count, cluster_count;
pf_kdtree_node_t **queue, *node;
queue_count = 0;
queue = calloc(self->node_count, sizeof(queue[0]));
// Put all the leaves in a queue
for (i = 0; i < self->node_count; i++)
{
node = self->nodes + i;
if (node->leaf)
{
node->cluster = -1;
assert(queue_count < self->node_count);
queue[queue_count++] = node;
// TESTING; remove
assert(node == pf_kdtree_find_node(self, self->root, node->key));
}
}
cluster_count = 0;
// Do connected components for each node
while (queue_count > 0)
{
node = queue[--queue_count];
// If this node has already been labelled, skip it
if (node->cluster >= 0)
continue;
// Assign a label to this cluster
node->cluster = cluster_count++;
// Recursively label nodes in this cluster
pf_kdtree_cluster_node(self, node, 0);
}
free(queue);
return;
}
////////////////////////////////////////////////////////////////////////////////
// Recursively label nodes in this cluster
void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth)
{
int i;
int nkey[3];
pf_kdtree_node_t *nnode;
for (i = 0; i < 3 * 3 * 3; i++)
{
nkey[0] = node->key[0] + (i / 9) - 1;
nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
nkey[2] = node->key[2] + ((i % 9) % 3) - 1;
nnode = pf_kdtree_find_node(self, self->root, nkey);
if (nnode == NULL)
continue;
assert(nnode->leaf);
// This node already has a label; skip it. The label should be
// consistent, however.
if (nnode->cluster >= 0)
{
assert(nnode->cluster == node->cluster);
continue;
}
// Label this node and recurse
nnode->cluster = node->cluster;
pf_kdtree_cluster_node(self, nnode, depth + 1);
}
return;
}
#ifdef INCLUDE_RTKGUI
////////////////////////////////////////////////////////////////////////////////
// Draw the tree
void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig)
{
if (self->root != NULL)
pf_kdtree_draw_node(self, self->root, fig);
return;
}
////////////////////////////////////////////////////////////////////////////////
// Recursively draw nodes
void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig)
{
double ox, oy;
char text[64];
if (node->leaf)
{
ox = (node->key[0] + 0.5) * self->size[0];
oy = (node->key[1] + 0.5) * self->size[1];
rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0);
//snprintf(text, sizeof(text), "%0.3f", node->value);
//rtk_fig_text(fig, ox, oy, 0.0, text);
snprintf(text, sizeof(text), "%d", node->cluster);
rtk_fig_text(fig, ox, oy, 0.0, text);
}
else
{
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
pf_kdtree_draw_node(self, node->children[0], fig);
pf_kdtree_draw_node(self, node->children[1], fig);
}
return;
}
#endif

147
navigations/amcl/src/amcl/pf/pf_pdf.c Executable file
View File

@@ -0,0 +1,147 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Useful pdf functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>
#include "amcl/pf/pf_pdf.h"
#include "portable_utils.hpp"
// Random number generator seed value
static unsigned int pf_pdf_seed;
/**************************************************************************
* Gaussian
*************************************************************************/
// Create a gaussian pdf
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
pf_matrix_t cd;
pf_pdf_gaussian_t *pdf;
pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
pdf->x = x;
pdf->cx = cx;
//pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
// Decompose the convariance matrix into a rotation
// matrix and a diagonal matrix.
pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
pdf->cd.v[0] = sqrt(cd.m[0][0]);
pdf->cd.v[1] = sqrt(cd.m[1][1]);
pdf->cd.v[2] = sqrt(cd.m[2][2]);
// Initialize the random number generator
//pdf->rng = gsl_rng_alloc(gsl_rng_taus);
//gsl_rng_set(pdf->rng, ++pf_pdf_seed);
srand48(++pf_pdf_seed);
return pdf;
}
// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
{
//gsl_rng_free(pdf->rng);
free(pdf);
return;
}
/*
// Compute the value of the pdf at some point [x].
double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
{
int i, j;
pf_vector_t z;
double zz, p;
z = pf_vector_sub(x, pdf->x);
zz = 0;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
return p;
}
*/
// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
{
int i, j;
pf_vector_t r;
pf_vector_t x;
// Generate a random vector
for (i = 0; i < 3; i++)
{
//r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
}
for (i = 0; i < 3; i++)
{
x.v[i] = pdf->x.v[i];
for (j = 0; j < 3; j++)
x.v[i] += pdf->cr.m[i][j] * r.v[j];
}
return x;
}
// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
// http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma)
{
double x1, x2, w, r;
do
{
do { r = drand48(); } while (r==0.0);
x1 = 2.0 * r - 1.0;
do { r = drand48(); } while (r==0.0);
x2 = 2.0 * r - 1.0;
w = x1*x1 + x2*x2;
} while(w > 1.0 || w==0.0);
return(sigma * x2 * sqrt(-2.0*log(w)/w));
}

276
navigations/amcl/src/amcl/pf/pf_vector.c Executable file
View File

@@ -0,0 +1,276 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Vector functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <math.h>
//#include <gsl/gsl_matrix.h>
//#include <gsl/gsl_eigen.h>
//#include <gsl/gsl_linalg.h>
#include "amcl/pf/pf_vector.h"
#include "amcl/pf/eig3.h"
// Return a zero vector
pf_vector_t pf_vector_zero()
{
pf_vector_t c;
c.v[0] = 0.0;
c.v[1] = 0.0;
c.v[2] = 0.0;
return c;
}
// Check for NAN or INF in any component
int pf_vector_finite(pf_vector_t a)
{
int i;
for (i = 0; i < 3; i++)
if (!isfinite(a.v[i]))
return 0;
return 1;
}
// Print a vector
void pf_vector_fprintf(pf_vector_t a, FILE *file, const char *fmt)
{
int i;
for (i = 0; i < 3; i++)
{
fprintf(file, fmt, a.v[i]);
fprintf(file, " ");
}
fprintf(file, "\n");
return;
}
// Simple vector addition
pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = a.v[0] + b.v[0];
c.v[1] = a.v[1] + b.v[1];
c.v[2] = a.v[2] + b.v[2];
return c;
}
// Simple vector subtraction
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = a.v[0] - b.v[0];
c.v[1] = a.v[1] - b.v[1];
c.v[2] = a.v[2] - b.v[2];
return c;
}
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
c.v[2] = b.v[2] + a.v[2];
c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
return c;
}
// Transform from global to local coords (a - b)
pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]);
c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]);
c.v[2] = a.v[2] - b.v[2];
c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
return c;
}
// Return a zero matrix
pf_matrix_t pf_matrix_zero()
{
int i, j;
pf_matrix_t c;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
c.m[i][j] = 0.0;
return c;
}
// Check for NAN or INF in any component
int pf_matrix_finite(pf_matrix_t a)
{
int i, j;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
if (!isfinite(a.m[i][j]))
return 0;
return 1;
}
// Print a matrix
void pf_matrix_fprintf(pf_matrix_t a, FILE *file, const char *fmt)
{
int i, j;
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
fprintf(file, fmt, a.m[i][j]);
fprintf(file, " ");
}
fprintf(file, "\n");
}
return;
}
/*
// Compute the matrix inverse
pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
{
double lndet;
int signum;
gsl_permutation *p;
gsl_matrix_view A, Ai;
pf_matrix_t ai;
A = gsl_matrix_view_array((double*) a.m, 3, 3);
Ai = gsl_matrix_view_array((double*) ai.m, 3, 3);
// Do LU decomposition
p = gsl_permutation_alloc(3);
gsl_linalg_LU_decomp(&A.matrix, p, &signum);
// Check for underflow
lndet = gsl_linalg_LU_lndet(&A.matrix);
if (lndet < -1000)
{
//printf("underflow in matrix inverse lndet = %f", lndet);
gsl_matrix_set_zero(&Ai.matrix);
}
else
{
// Compute inverse
gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix);
}
gsl_permutation_free(p);
if (det)
*det = exp(lndet);
return ai;
}
*/
// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
// matrix [d] such that a = r d r^T.
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
{
int i, j;
/*
gsl_matrix *aa;
gsl_vector *eval;
gsl_matrix *evec;
gsl_eigen_symmv_workspace *w;
aa = gsl_matrix_alloc(3, 3);
eval = gsl_vector_alloc(3);
evec = gsl_matrix_alloc(3, 3);
*/
double aa[3][3];
double eval[3];
double evec[3][3];
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
//gsl_matrix_set(aa, i, j, a.m[i][j]);
aa[i][j] = a.m[i][j];
}
}
// Compute eigenvectors/values
/*
w = gsl_eigen_symmv_alloc(3);
gsl_eigen_symmv(aa, eval, evec, w);
gsl_eigen_symmv_free(w);
*/
eigen_decomposition(aa,evec,eval);
*d = pf_matrix_zero();
for (i = 0; i < 3; i++)
{
//d->m[i][i] = gsl_vector_get(eval, i);
d->m[i][i] = eval[i];
for (j = 0; j < 3; j++)
{
//r->m[i][j] = gsl_matrix_get(evec, i, j);
r->m[i][j] = evec[i][j];
}
}
//gsl_matrix_free(evec);
//gsl_vector_free(eval);
//gsl_matrix_free(aa);
return;
}

510
navigations/amcl/src/amcl/sensors/amcl_laser.cpp Executable file
View File

@@ -0,0 +1,510 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: AMCL laser routines
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_laser.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include <sys/types.h> // required by Darwin
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include "amcl/sensors/amcl_laser.h"
using namespace amcl;
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor(),
max_samples(0), max_obs(0),
temp_obs(NULL)
{
this->time = 0.0;
this->max_beams = max_beams;
this->map = map;
return;
}
AMCLLaser::~AMCLLaser()
{
if(temp_obs){
for(int k=0; k < max_samples; k++){
delete [] temp_obs[k];
}
delete []temp_obs;
}
}
void
AMCLLaser::SetModelBeam(double z_hit,
double z_short,
double z_max,
double z_rand,
double sigma_hit,
double lambda_short,
double chi_outlier)
{
this->model_type = LASER_MODEL_BEAM;
this->z_hit = z_hit;
this->z_short = z_short;
this->z_max = z_max;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
this->lambda_short = lambda_short;
this->chi_outlier = chi_outlier;
}
void
AMCLLaser::SetModelLikelihoodField(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist)
{
this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
this->z_hit = z_hit;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
map_update_cspace(this->map, max_occ_dist);
}
void
AMCLLaser::SetModelLikelihoodFieldProb(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist,
bool do_beamskip,
double beam_skip_distance,
double beam_skip_threshold,
double beam_skip_error_threshold)
{
this->model_type = LASER_MODEL_LIKELIHOOD_FIELD_PROB;
this->z_hit = z_hit;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
this->do_beamskip = do_beamskip;
this->beam_skip_distance = beam_skip_distance;
this->beam_skip_threshold = beam_skip_threshold;
this->beam_skip_error_threshold = beam_skip_error_threshold;
map_update_cspace(this->map, max_occ_dist);
}
////////////////////////////////////////////////////////////////////////////////
// Apply the laser sensor model
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
if (this->max_beams < 2)
return false;
// Apply the laser sensor model
if(this->model_type == LASER_MODEL_BEAM)
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);
else
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
return true;
}
////////////////////////////////////////////////////////////////////////////////
// Determine the probability for the given pose
double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double map_range;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
step = (data->range_count - 1) / (self->max_beams - 1);
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// Compute the range according to the map
map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
pose.v[2] + obs_bearing, data->range_max);
pz = 0.0;
// Part 1: good, but noisy, hit
z = obs_range - map_range;
pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
// Part 2: short reading from unexpected obstacle (e.g., a person)
if(z < 0)
pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);
// Part 3: Failure to detect obstacle, reported as max-range
if(obs_range == data->range_max)
pz += self->z_max * 1.0;
// Part 4: Random measurements
if(obs_range < data->range_max)
pz += self->z_rand * 1.0/data->range_max;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return(total_weight);
}
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
step = (data->range_count - 1) / (self->max_beams - 1);
// Step size must be at least 1
if(step < 1)
step = 1;
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max)
continue;
// Check for NaN
if(obs_range != obs_range)
continue;
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj))
z = self->map->max_occ_dist;
else
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return(total_weight);
}
double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double log_p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
step = ceil((data->range_count) / static_cast<double>(self->max_beams));
// Step size must be at least 1
if(step < 1)
step = 1;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom);
//Beam skipping - ignores beams for which a majority of particles do not agree with the map
//prevents correct particles from getting down weighted because of unexpected obstacles
//such as humans
bool do_beamskip = self->do_beamskip;
double beam_skip_distance = self->beam_skip_distance;
double beam_skip_threshold = self->beam_skip_threshold;
//we only do beam skipping if the filter has converged
if(do_beamskip && !set->converged){
do_beamskip = false;
}
//we need a count the no of particles for which the beam agreed with the map
int *obs_count = new int[self->max_beams]();
//we also need a mask of which observations to integrate (to decide which beams to integrate to all particles)
bool *obs_mask = new bool[self->max_beams]();
int beam_ind = 0;
//realloc indicates if we need to reallocate the temp data structure needed to do beamskipping
bool realloc = false;
if(do_beamskip){
if(self->max_obs < self->max_beams){
realloc = true;
}
if(self->max_samples < set->sample_count){
realloc = true;
}
if(realloc){
self->reallocTempData(set->sample_count, self->max_beams);
fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs);
}
}
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
log_p = 0;
beam_ind = 0;
for (i = 0; i < data->range_count; i += step, beam_ind++)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max){
continue;
}
// Check for NaN
if(obs_range != obs_range){
continue;
}
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj)){
pz += self->z_hit * max_dist_prob;
}
else{
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
if(z < beam_skip_distance){
obs_count[beam_ind] += 1;
}
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
}
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
assert(pz <= 1.0);
assert(pz >= 0.0);
// TODO: outlier rejection for short readings
if(!do_beamskip){
log_p += log(pz);
}
else{
self->temp_obs[j][beam_ind] = pz;
}
}
if(!do_beamskip){
sample->weight *= exp(log_p);
total_weight += sample->weight;
}
}
if(do_beamskip){
int skipped_beam_count = 0;
for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
if((obs_count[beam_ind] / static_cast<double>(set->sample_count)) > beam_skip_threshold){
obs_mask[beam_ind] = true;
}
else{
obs_mask[beam_ind] = false;
skipped_beam_count++;
}
}
//we check if there is at least a critical number of beams that agreed with the map
//otherwise it probably indicates that the filter converged to a wrong solution
//if that's the case we integrate all the beams and hope the filter might converge to
//the right solution
bool error = false;
if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){
fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold));
error = true;
}
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
log_p = 0;
for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
if(error || obs_mask[beam_ind]){
log_p += log(self->temp_obs[j][beam_ind]);
}
}
sample->weight *= exp(log_p);
total_weight += sample->weight;
}
}
delete [] obs_count;
delete [] obs_mask;
return(total_weight);
}
void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){
if(temp_obs){
for(int k=0; k < max_samples; k++){
delete [] temp_obs[k];
}
delete []temp_obs;
}
max_obs = new_max_obs;
max_samples = fmax(max_samples, new_max_samples);
temp_obs = new double*[max_samples]();
for(int k=0; k < max_samples; k++){
temp_obs[k] = new double[max_obs]();
}
}

310
navigations/amcl/src/amcl/sensors/amcl_odom.cpp Executable file
View File

@@ -0,0 +1,310 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: AMCL odometry routines
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <sys/types.h> // required by Darwin
#include <math.h>
#include "amcl/sensors/amcl_odom.h"
using namespace amcl;
static double
normalize(double z)
{
return atan2(sin(z),cos(z));
}
static double
angle_diff(double a, double b)
{
double d1, d2;
a = normalize(a);
b = normalize(b);
d1 = a-b;
d2 = 2*M_PI - fabs(d1);
if(d1 > 0)
d2 *= -1.0;
if(fabs(d1) < fabs(d2))
return(d1);
else
return(d2);
}
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLOdom::AMCLOdom() : AMCLSensor()
{
this->time = 0.0;
}
void
AMCLOdom::SetModelDiff(double alpha1,
double alpha2,
double alpha3,
double alpha4)
{
this->model_type = ODOM_MODEL_DIFF;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
}
void
AMCLOdom::SetModelOmni(double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5)
{
this->model_type = ODOM_MODEL_OMNI;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
this->alpha5 = alpha5;
}
void
AMCLOdom::SetModel( odom_model_t type,
double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5 )
{
this->model_type = type;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
this->alpha5 = alpha5;
}
////////////////////////////////////////////////////////////////////////////////
// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
AMCLOdomData *ndata;
ndata = (AMCLOdomData*) data;
// Compute the new sample poses
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
switch( this->model_type )
{
case ODOM_MODEL_OMNI:
{
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot = ndata->delta.v[2];
// Precompute a couple of things
double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
alpha1 * (delta_rot*delta_rot));
double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
alpha2 * (delta_trans*delta_trans));
double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
alpha5 * (delta_trans*delta_trans));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]) + sample->pose.v[2];
double cs_bearing = cos(delta_bearing);
double sn_bearing = sin(delta_bearing);
// Sample pose differences
delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
// Apply sampled update to particle pose
sample->pose.v[0] += (delta_trans_hat * cs_bearing +
delta_strafe_hat * sn_bearing);
sample->pose.v[1] += (delta_trans_hat * sn_bearing -
delta_strafe_hat * cs_bearing);
sample->pose.v[2] += delta_rot_hat ;
}
}
break;
case ODOM_MODEL_DIFF:
{
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
// Sample pose differences
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans));
delta_trans_hat = delta_trans -
pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise);
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans));
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
break;
case ODOM_MODEL_OMNI_CORRECTED:
{
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot = ndata->delta.v[2];
// Precompute a couple of things
double trans_hat_stddev = sqrt( alpha3 * (delta_trans*delta_trans) +
alpha4 * (delta_rot*delta_rot) );
double rot_hat_stddev = sqrt( alpha1 * (delta_rot*delta_rot) +
alpha2 * (delta_trans*delta_trans) );
double strafe_hat_stddev = sqrt( alpha4 * (delta_rot*delta_rot) +
alpha5 * (delta_trans*delta_trans) );
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]) + sample->pose.v[2];
double cs_bearing = cos(delta_bearing);
double sn_bearing = sin(delta_bearing);
// Sample pose differences
delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
// Apply sampled update to particle pose
sample->pose.v[0] += (delta_trans_hat * cs_bearing +
delta_strafe_hat * sn_bearing);
sample->pose.v[1] += (delta_trans_hat * sn_bearing -
delta_strafe_hat * cs_bearing);
sample->pose.v[2] += delta_rot_hat ;
}
}
break;
case ODOM_MODEL_DIFF_CORRECTED:
{
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
// Sample pose differences
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(sqrt(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans)));
delta_trans_hat = delta_trans -
pf_ran_gaussian(sqrt(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise));
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(sqrt(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans)));
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
break;
}
return true;
}

95
navigations/amcl/src/amcl/sensors/amcl_sensor.cpp Executable file
View File

@@ -0,0 +1,95 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: AMCL sensor
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_sensor.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include "amcl/sensors/amcl_sensor.h"
using namespace amcl;
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLSensor::AMCLSensor()
{
return;
}
AMCLSensor::~AMCLSensor()
{
}
////////////////////////////////////////////////////////////////////////////////
// Apply the action model
bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Initialize the filter
bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data)
{
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Apply the sensor model
bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
return false;
}
#ifdef INCLUDE_RTKGUI
////////////////////////////////////////////////////////////////////////////////
// Setup the GUI
void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
{
return;
}
////////////////////////////////////////////////////////////////////////////////
// Shutdown the GUI
void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
{
return;
}
////////////////////////////////////////////////////////////////////////////////
// Draw sensor data
void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data)
{
return;
}
#endif

1656
navigations/amcl/src/amcl_node.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

28
navigations/amcl/src/include/portable_utils.hpp Executable file
View File

@@ -0,0 +1,28 @@
#ifndef PORTABLE_UTILS_H
#define PORTABLE_UTILS_H
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
#ifndef HAVE_DRAND48
// Some system (e.g., Windows) doesn't come with drand48(), srand48().
// Use rand, and srand for such system.
static double drand48(void)
{
return ((double)rand())/RAND_MAX;
}
static void srand48(long int seedval)
{
srand(seedval);
}
#endif
#ifdef __cplusplus
}
#endif
#endif

101
navigations/amcl/test/basic_localization.py Executable file
View File

@@ -0,0 +1,101 @@
#!/usr/bin/env python
from __future__ import print_function
import sys
import time
from math import fmod, pi
import unittest
import rospy
import rostest
import tf2_py as tf2
import tf2_ros
import PyKDL
from std_srvs.srv import Empty
class TestBasicLocalization(unittest.TestCase):
def setUp(self):
self.tf = None
self.target_x = None
self.target_y = None
self.target_a = None
self.tfBuffer = None
def print_pose_diff(self):
a_curr = self.compute_angle()
a_diff = self.wrap_angle(a_curr - self.target_a)
print('Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr))
print('Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a))
print('Diff:\t %16.6f %16.6f %16.6f' % (
abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff))
def get_pose(self):
try:
tf_stamped = self.tfBuffer.lookup_transform("map", "base_link", rospy.Time())
self.tf = tf_stamped.transform
self.print_pose_diff()
except (tf2.LookupException, tf2.ExtrapolationException):
pass
@staticmethod
def wrap_angle(angle):
"""
Wrap angle to [-pi, pi)
:param angle: Angle to be wrapped
:return: wrapped angle
"""
angle += pi
while angle < 0:
angle += 2*pi
return fmod(angle, 2*pi) - pi
def compute_angle(self):
rot = self.tf.rotation
a_curr = PyKDL.Rotation.Quaternion(rot.x, rot.y, rot.z, rot.w).GetRPY()[2]
a_diff = self.wrap_angle(a_curr - self.target_a)
return self.target_a + a_diff
def test_basic_localization(self):
global_localization = int(sys.argv[1])
self.target_x = float(sys.argv[2])
self.target_y = float(sys.argv[3])
self.target_a = self.wrap_angle(float(sys.argv[4]))
tolerance_d = float(sys.argv[5])
tolerance_a = float(sys.argv[6])
target_time = float(sys.argv[7])
rospy.init_node('test', anonymous=True)
while rospy.rostime.get_time() == 0.0:
print('Waiting for initial time publication')
time.sleep(0.1)
if global_localization == 1:
print('Waiting for service global_localization')
rospy.wait_for_service('global_localization')
global_localization = rospy.ServiceProxy('global_localization', Empty)
global_localization()
start_time = rospy.rostime.get_time()
self.tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(self.tfBuffer)
while (rospy.rostime.get_time() - start_time) < target_time:
print('Waiting for end time %.6f (current: %.6f)' % (target_time, (rospy.rostime.get_time() - start_time)))
self.get_pose()
time.sleep(0.1)
print("Final pose:")
self.get_pose()
a_curr = self.compute_angle()
self.assertNotEqual(self.tf, None)
self.assertAlmostEqual(self.tf.translation.x, self.target_x, delta=tolerance_d)
self.assertAlmostEqual(self.tf.translation.y, self.target_y, delta=tolerance_d)
self.assertAlmostEqual(a_curr, self.target_a, delta=tolerance_a)
if __name__ == '__main__':
rostest.run('amcl', 'amcl_localization', TestBasicLocalization, sys.argv)

51
navigations/amcl/test/basic_localization_stage.xml Executable file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="47.443"/>
<param name="initial_pose_y" value="21.421"/>
<param name="initial_pose_a" value="-1.003"/>
</node>
<test time-limit="180" test-name="basic_localization_stage" pkg="amcl"
type="basic_localization.py" args="0 30.329 34.644 3.142 0.75 0.75 90.0"/>
</launch>

48
navigations/amcl/test/global_localization_stage.xml Executable file
View File

@@ -0,0 +1,48 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/global_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
<test time-limit="180" test-name="global_localization_stage" pkg="amcl"
type="basic_localization.py" args="1 10.00 9.67 6.21 0.25 0.25 28.0" />
</launch>

51
navigations/amcl/test/rosie_multilaser.xml Executable file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<!-- Setting pose: 42.378 17.730 1.583
Setting pose: 33.118 34.530 -0.519
103.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/rosie_localization_stage.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="42.378"/>
<param name="initial_pose_y" value="17.730"/>
<param name="initial_pose_a" value="1.583"/>
</node>
<test time-limit="180" test-name="basic_localization_stage_rosie" pkg="amcl"
type="basic_localization.py" args="0 33.12 34.53 -0.52 0.75 0.4 103.5"/>
</launch>

54
navigations/amcl/test/set_initial_pose.xml Executable file
View File

@@ -0,0 +1,54 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<!--
<param name="initial_pose_x" value="47.443"/>
<param name="initial_pose_y" value="21.421"/>
<param name="initial_pose_a" value="-1.003"/>
-->
</node>
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
type="basic_localization.py" args="0 30.33 34.64 3.14 0.75 0.75 90.0"/>
</launch>

58
navigations/amcl/test/set_initial_pose_delayed.xml Executable file
View File

@@ -0,0 +1,58 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
<!-- The bagfile starts at 34.6s. We try to send the same initial pose as above with stamp 34.8 at 5 seconds
into the bagfile (at 39.6s). AMCL should add the robot motion from 34.8 to 39.6 to that initial pose
before using it, so there should be no mis-localization caused by this. -->
<node name="delayed_pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003 34.8 39.6"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<!--
<param name="initial_pose_x" value="47.443"/>
<param name="initial_pose_y" value="21.421"/>
<param name="initial_pose_a" value="-1.003"/>
-->
</node>
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
type="basic_localization.py" args="0 30.33 34.64 3.14 0.75 0.75 90.0"/>
</launch>

46
navigations/amcl/test/set_pose.py Executable file
View File

@@ -0,0 +1,46 @@
#!/usr/bin/env python
import rospy
import math
import PyKDL
from geometry_msgs.msg import PoseWithCovarianceStamped
class PoseSetter(rospy.SubscribeListener):
def __init__(self, pose, stamp, publish_time):
self.pose = pose
self.stamp = stamp
self.publish_time = publish_time
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
p = PoseWithCovarianceStamped()
p.header.frame_id = "map"
p.header.stamp = self.stamp
p.pose.pose.position.x = self.pose[0]
p.pose.pose.position.y = self.pose[1]
(p.pose.pose.orientation.x,
p.pose.pose.orientation.y,
p.pose.pose.orientation.z,
p.pose.pose.orientation.w) = PyKDL.Rotation.RPY(0, 0, self.pose[2]).GetQuaternion()
p.pose.covariance[6*0+0] = 0.5 * 0.5
p.pose.covariance[6*1+1] = 0.5 * 0.5
p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
# wait for the desired publish time
while rospy.get_rostime() < self.publish_time:
rospy.sleep(0.01)
peer_publish(p)
if __name__ == '__main__':
pose = list(map(float, rospy.myargv()[1:4]))
t_stamp = rospy.Time()
t_publish = rospy.Time()
if len(rospy.myargv()) > 4:
t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4]))
if len(rospy.myargv()) > 5:
t_publish = rospy.Time.from_sec(float(rospy.myargv()[5]))
rospy.init_node('pose_setter', anonymous=True)
rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec()))
pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1)
rospy.spin()

51
navigations/amcl/test/small_loop_crazy_driving_prg.xml Executable file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_crazy_driving_prg_indexed.bag"/>
<node name="map_sever" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="14.016"/>
<param name="initial_pose_y" value="25.7500"/>
<param name="initial_pose_a" value="-3.0"/>
<param name="initial_cov_xx" value="0.1"/>
<param name="initial_cov_yy" value="0.1"/>
<param name="initial_cov_aa" value="0.1"/>
</node>
<test time-limit="180" test-name="small_loop_crazy_driving_prg" pkg="amcl"
type="basic_localization.py" args="0 14.05 28.91 4.28 0.75 0.75 90.0"/>
</launch>

51
navigations/amcl/test/small_loop_crazy_driving_prg_corrected.xml Executable file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_crazy_driving_prg_indexed.bag"/>
<node name="map_sever" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni-corrected"/>
<param name="odom_alpha1" value="0.005"/>
<param name="odom_alpha2" value="0.005"/>
<param name="odom_alpha3" value="0.01"/>
<param name="odom_alpha4" value="0.005"/>
<param name="odom_alpha5" value="0.003"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="14.016"/>
<param name="initial_pose_y" value="25.7500"/>
<param name="initial_pose_a" value="-3.0"/>
<param name="initial_cov_xx" value="0.1"/>
<param name="initial_cov_yy" value="0.1"/>
<param name="initial_cov_aa" value="0.1"/>
</node>
<test time-limit="180" test-name="small_loop_crazy_driving_prg" pkg="amcl"
type="basic_localization.py" args="0 14.05 28.91 4.28 0.75 0.75 90.0"/>
</launch>

48
navigations/amcl/test/small_loop_prf.xml Executable file
View File

@@ -0,0 +1,48 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_prf_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="14.099"/>
<param name="initial_pose_y" value="21.063"/>
<param name="initial_pose_a" value="-0.006"/>
</node>
<test time-limit="180" test-name="basic_localization_small_loop_prf" pkg="amcl"
type="basic_localization.py" args="0 13.94 23.02 4.72 0.75 0.75 66.0"/>
</launch>

41
navigations/amcl/test/texas_greenroom_loop.xml Executable file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_greenroom_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha1" value="0.1"/>
<param name="odom_alpha2" value="0.3"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="14.049"/>
<param name="initial_pose_y" value="24.234"/>
<param name="initial_pose_a" value="-1.517"/>
</node>
<test time-limit="180" test-name="texas_greenroom_loop" pkg="amcl"
type="basic_localization.py" args="0 13.87 24.07 4.65 0.75 0.75 89.0"/>
</launch>

41
navigations/amcl/test/texas_greenroom_loop_corrected.xml Executable file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_greenroom_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha1" value="0.04"/>
<param name="odom_alpha2" value="0.6"/>
<param name="odom_alpha3" value="0.3"/>
<param name="odom_alpha4" value="0.04"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="14.049"/>
<param name="initial_pose_y" value="24.234"/>
<param name="initial_pose_a" value="-1.517"/>
</node>
<test time-limit="180" test-name="texas_greenroom_loop_corrected" pkg="amcl"
type="basic_localization.py" args="0 13.87 24.07 4.65 0.75 0.75 89.0"/>
</launch>

41
navigations/amcl/test/texas_willow_hallway_loop.xml Executable file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_willow_hallway_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha1" value="0.1"/>
<param name="odom_alpha2" value="0.3"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="31.872"/>
<param name="initial_pose_y" value="31.557"/>
<param name="initial_pose_a" value="0.0"/>
</node>
<test time-limit="350" test-name="texas_willow_hallway_loop" pkg="amcl"
type="basic_localization.py" args="0 32.36 31.44 6.18 0.75 0.75 250.0"/>
</launch>

41
navigations/amcl/test/texas_willow_hallway_loop_corrected.xml Executable file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_willow_hallway_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="odom_alpha1" value="0.04"/>
<param name="odom_alpha2" value="0.6"/>
<param name="odom_alpha3" value="0.3"/>
<param name="odom_alpha4" value="0.04"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="31.872"/>
<param name="initial_pose_y" value="31.557"/>
<param name="initial_pose_a" value="0.0"/>
</node>
<test time-limit="350" test-name="texas_willow_hallway_loop_corrected" pkg="amcl"
type="basic_localization.py" args="0 32.36 31.44 6.18 0.75 0.75 250.0"/>
</launch>

214
navigations/base_local_planner/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,214 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package base_local_planner
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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)
-------------------
* Commit 89a8593 removed footprint scaling. This brings it back. (`#886 <https://github.com/ros-planning/navigation/issues/886>`_) (`#1204 <https://github.com/ros-planning/navigation/issues/1204>`_)
Co-authored-by: Frank Höller <frank.hoeller@fkie.fraunhofer.de>
* Contributors: Michael Ferguson
1.17.1 (2020-08-27)
-------------------
* occdist_scale should not be scaled by the costmap resolution as it doesn't multiply a value that includes a distance. (`#1000 <https://github.com/ros-planning/navigation/issues/1000>`_)
* Contributors: wjwagner
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
* increase required cmake version
* Contributors: Michael Ferguson
1.16.6 (2020-03-18)
-------------------
* Fix Unknown CMake command check_include_file (navfn & base_local_planner) (`#975 <https://github.com/ros-planning/navigation/issues/975>`_)
* Contributors: Sam Pfeiffer
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)
-------------------
* Fixes gdist- and pdist_scale node paramter names (`#936 <https://github.com/cobalt-robotics/navigation/issues/936>`_)
Renames goal and path distance dynamic reconfigure parameter
names in the cfg file in order to actually make the parameters
used by the trajectory planner changeable.
Fixes `#935 <https://github.com/cobalt-robotics/navigation/issues/935>`_
* don't include a main() function in base_local_planner library (`#969 <https://github.com/cobalt-robotics/navigation/issues/969>`_)
* [Windows][melodic] Navigation (except for map_server and amcl) Windows build bring up (`#851 <https://github.com/cobalt-robotics/navigation/issues/851>`_)
* Contributors: David Leins, Sean Yen, ipa-fez
1.16.3 (2019-11-15)
-------------------
* Merge branch 'melodic-devel' into layer_clear_area-melodic
* Provide different negative values for unknown and out-of-map costs (`#833 <https://github.com/ros-planning/navigation/issues/833>`_)
* Merge pull request `#857 <https://github.com/ros-planning/navigation/issues/857>`_ from jspricke/add_include
Add missing header
* Add missing header
* [kinetic] Fix for adjusting plan resolution (`#819 <https://github.com/ros-planning/navigation/issues/819>`_)
* Fix for adjusting plan resolution
* Simpler math expression
* Contributors: David V. Lu!!, Jochen Sprickerhof, Jorge Santos Simón, Michael Ferguson, Steven Macenski
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
* add explicit sensor_msgs, tf2 depends for base_local_planner
* Contributors: Michael Ferguson
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Remove PCL `#765 <https://github.com/ros-planning/navigation/issues/765>`_
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Fix trajectory obstacle scoring in dwa_local_planner.
* Make trajectory scoring scales consistent.
* unify parameter names between base_local_planner and dwa_local_planner
addresses parts of `#90 <https://github.com/ros-planning/navigation/issues/90>`_
* fix param to min_in_place_vel_theta, closes `#487 <https://github.com/ros-planning/navigation/issues/487>`_
* add const to getLocalPlane, fixes `#709 <https://github.com/ros-planning/navigation/issues/709>`_
* Merge pull request `#732 <https://github.com/ros-planning/navigation/issues/732>`_ from marting87/small_typo_fixes
Small typo fixes in ftrajectory_planner_ros and robot_pose_ekf
* Fixed typos for base_local_planner
* Contributors: Alexander Moriarty, David V. Lu, Martin Ganeff, Michael Ferguson, Pavlo Kolomiiets, Rein Appeldoorn, Vincent Rabaud, moriarty
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)
* CostmapModel: Make lineCost and pointCost public (`#658 <https://github.com/ros-planning/navigation/issues/658>`_)
Make the methods `lineCost` and `pointCost` of the CostmapModel class
public so they can be used outside of the class.
Both methods are not changing the instance, so this should not cause any
problems. To emphasise their constness, add the actual `const` keyword.
* 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, Felix Widmaier, Michael Ferguson
1.15.1 (2017-08-14)
-------------------
1.15.0 (2017-08-07)
-------------------
* set message_generation build and runtime dependency
* convert packages to format2
* cleaner logic, fixes `#156 <https://github.com/ros-planning/navigation/issues/156>`_
* Merge pull request `#596 <https://github.com/ros-planning/navigation/issues/596>`_ from ros-planning/lunar_548
* Add cost function to prevent unnecessary spinning
* 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>`_)
* make rostest in CMakeLists optional (`ros/rosdistro#3010 <https://github.com/ros/rosdistro/issues/3010>`_)
* remove GCC warnings
* Contributors: Lukas Bulwahn, Martin Günther, Michael Ferguson, Mikael Arguedas, Morgan Quigley, Vincent Rabaud, lengly
1.14.0 (2016-05-20)
-------------------
1.13.1 (2015-10-29)
-------------------
* base_local_planner: some fixes in goal_functions
* Merge pull request `#348 <https://github.com/ros-planning/navigation/issues/348>`_ from mikeferguson/trajectory_planner_fixes
* fix stuck_left/right calculation
* fix calculation of heading diff
* Contributors: Gael Ecorchard, Michael Ferguson
1.13.0 (2015-03-17)
-------------------
* remove previously deprecated param
* Contributors: Michael Ferguson
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)
--------------------
* Fixed setting child_frame_id in base_local_planner::OdometryHelperRos
* Contributors: Mani Monajjemi
1.11.13 (2014-10-02)
--------------------
1.11.12 (2014-10-01)
--------------------
* Bugfix uninitialised occ_cost variable usage
This fixes `#256 <https://github.com/ros-planning/navigation/issues/256>`_.
* base_local_planner: adds waitForTransform
* Fixed issue causing trajectory planner returning false to isGoalReach ed even when it's control thread finishes executing
* Contributors: Daniel Stonier, Marcus Liebhardt, hes3pal
1.11.11 (2014-07-23)
--------------------
* Minor code cleanup
* Contributors: Enrique Fernández Perdomo
1.11.10 (2014-06-25)
--------------------
* Remove unnecessary colons
* renames acc_lim_th to acc_lim_theta, add warning if using acc_lim_th
* uses odom child_frame_id to set robot_vel frame_id
* Contributors: David Lu!!, Michael Ferguson, Enrique Fernández Perdomo
1.11.9 (2014-06-10)
-------------------
* uses ::hypot(x, y) instead of sqrt(x*x, y*y)
* No need to use `limits->`
* Contributors: Enrique Fernández Perdomo
1.11.8 (2014-05-21)
-------------------
1.11.7 (2014-05-21)
-------------------
* fixes latch_xy_goal_tolerance param not taken
* update build to find eigen using cmake_modules
* Trajectory: fix constness of getter methods
* Use hypot() instead of sqrt(x*x, y*y)
* Fix bug in distance calculation for trajectory rollout
* Some documentation fixes in SimpleTrajectoryGenerator
* Contributors: Michael Ferguson, Siegfried-A. Gevatter Pujals, enriquefernandez
1.11.5 (2014-01-30)
-------------------
* Merge pull request `#152 <https://github.com/ros-planning/navigation/issues/152>`_ from KaijenHsiao/hydro-devel
uncommented trajectory_planner_ros from catkin_package LIBRARIES so other packages can find it
* Fix negative score bug, add ability to sum scores
* Ignore pyc files from running in devel
* Correct type of prefer_forward penalty member variable
* uncommented trajectory_planner_ros from catkin_package LIBRARIES so other packages can find it
* Better handling of frame param in MapGridVisualizer
* check for CATKIN_ENABLE_TESTING
* Change maintainer from Hersh to Lu
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* Changed new Odom-Helper::initialize() function to setOdomTopic().
* Converted to a pointcloud pointer in Observation in more places.

165
navigations/base_local_planner/CMakeLists.txt Executable file
View File

@@ -0,0 +1,165 @@
cmake_minimum_required(VERSION 3.0.2)
project(base_local_planner)
include(CheckIncludeFile)
find_package(catkin REQUIRED
COMPONENTS
angles
cmake_modules
costmap_2d
dynamic_reconfigure
geometry_msgs
message_generation
nav_core
nav_msgs
pluginlib
roscpp
rospy
sensor_msgs
std_msgs
tf2
tf2_geometry_msgs
tf2_ros
voxel_grid
)
find_package(Boost REQUIRED
COMPONENTS
thread
)
find_package(Eigen3 REQUIRED)
remove_definitions(-DDISABLE_LIBUSB-1.0)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
catkin_python_setup()
# messages
add_message_files(
DIRECTORY msg
FILES
Position2DInt.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)
# dynamic reconfigure
generate_dynamic_reconfigure_options(
cfg/BaseLocalPlanner.cfg
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES
base_local_planner
trajectory_planner_ros
CATKIN_DEPENDS
angles
costmap_2d
dynamic_reconfigure
geometry_msgs
message_runtime
nav_core
nav_msgs
pluginlib
roscpp
sensor_msgs
std_msgs
tf2
tf2_ros
voxel_grid
)
check_include_file(sys/time.h HAVE_SYS_TIME_H)
if (HAVE_SYS_TIME_H)
add_definitions(-DHAVE_SYS_TIME_H)
endif (HAVE_SYS_TIME_H)
#uncomment for profiling
#set(ROS_COMPILE_FLAGS "-g -pg" ${ROS_COMPILE_FLAGS})
#set(ROS_LINK_FLAGS "-g -pg" ${ROS_LINK_FLAGS})
#set(ROS_COMPILE_FLAGS "-g" ${ROS_COMPILE_FLAGS})
#set(ROS_LINK_FLAGS "-g" ${ROS_LINK_FLAGS})
add_library(base_local_planner
src/footprint_helper.cpp
src/goal_functions.cpp
src/map_cell.cpp
src/map_grid.cpp
src/map_grid_visualizer.cpp
src/map_grid_cost_function.cpp
src/latched_stop_rotate_controller.cpp
src/local_planner_util.cpp
src/odometry_helper_ros.cpp
src/obstacle_cost_function.cpp
src/oscillation_cost_function.cpp
src/prefer_forward_cost_function.cpp
src/point_grid.cpp
src/costmap_model.cpp
src/simple_scored_sampling_planner.cpp
src/simple_trajectory_generator.cpp
src/trajectory.cpp
src/twirling_cost_function.cpp
src/voxel_grid_model.cpp)
add_dependencies(base_local_planner base_local_planner_gencfg)
add_dependencies(base_local_planner base_local_planner_generate_messages_cpp)
add_dependencies(base_local_planner nav_msgs_generate_messages_cpp)
target_link_libraries(base_local_planner
${catkin_LIBRARIES}
${Boost_LIBRARIES}
${Eigen_LIBRARIES}
)
add_library(trajectory_planner_ros
src/trajectory_planner.cpp
src/trajectory_planner_ros.cpp)
add_dependencies(trajectory_planner_ros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(trajectory_planner_ros
base_local_planner)
add_executable(point_grid src/point_grid_node.cpp)
add_dependencies(point_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(point_grid ${catkin_LIBRARIES} base_local_planner)
install(TARGETS
base_local_planner
trajectory_planner_ros
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(FILES blp_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
PATTERN ".svn" EXCLUDE
)
if(CATKIN_ENABLE_TESTING)
find_package(rostest)
catkin_add_gtest(base_local_planner_utest
test/gtest_main.cpp
test/utest.cpp
test/velocity_iterator_test.cpp
test/footprint_helper_test.cpp
test/trajectory_generator_test.cpp
test/map_grid_test.cpp)
target_link_libraries(base_local_planner_utest
base_local_planner trajectory_planner_ros
)
catkin_add_gtest(line_iterator
test/line_iterator_test.cpp)
endif()

7
navigations/base_local_planner/blp_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libtrajectory_planner_ros">
<class name="base_local_planner/TrajectoryPlannerROS" type="base_local_planner::TrajectoryPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
<description>
A implementation of a local planner using either a DWA or Trajectory Rollout approach based on configuration parameters.
</description>
</class>
</library>

57
navigations/base_local_planner/cfg/BaseLocalPlanner.cfg Executable file
View File

@@ -0,0 +1,57 @@
#!/usr/bin/env python
PACKAGE = 'base_local_planner'
from math import pi
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t, str_t
gen = ParameterGenerator()
# gen.add("inscribed_radius", double_t, 0, "The radius of the inscribed circle of the robot", 1, 0)
# gen.add("circumscribed_radius", double_t, 0, "The radius of the circumscribed circle of the robot", 1, 0)
gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 2.5, 0, 20.0)
gen.add("acc_lim_y", double_t, 0, "The acceleration limit of the robot in the y direction", 2.5, 0, 20.0)
gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in the theta direction", 3.2, 0, 20.0)
gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55, 0, 20.0)
gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0, 0, 20.0)
gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0, 20.0)
gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", -1.0, -20.0, 0.0)
gen.add("min_in_place_vel_theta", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.4, 0, 20.0)
gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5)
gen.add("angular_sim_granularity", double_t, 0, "The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things", 0.025, 0, pi/2)
gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0, 5)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0, 5)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0, 5)
gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0, 5)
gen.add("escape_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.10, 0, 5)
gen.add("escape_reset_theta", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", pi/2, 0, 5)
gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 20, 1, 300)
gen.add("vtheta_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1, 300)
gen.add("heading_lookahead", double_t, 0, "How far the robot should look ahead of itself when differentiating between different rotational velocities", 0.325, 0, 5)
gen.add("holonomic_robot", bool_t, 0, "Set this to true if the robot being controlled can take y velocities and false otherwise", True)
gen.add("escape_vel", double_t, 0, "The velocity to use while backing up", -0.1, -2, 2)
gen.add("dwa", bool_t, 0, "Set this to true to use the Dynamic Window Approach, false to use acceleration limits", False)
gen.add("heading_scoring", bool_t, 0, "Set this to true to use the Dynamic Window Approach, false to use acceleration limits", False)
gen.add("heading_scoring_timestep", double_t, 0, "How far to look ahead in time when we score heading based trajectories", 0.1, 0, 1)
gen.add("simple_attractor", bool_t, 0, "Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation", False)
gen.add("y_vels", str_t, 0, "A comma delimited list of the y velocities the controller will explore", "-0.3,-0.1,0.1,-0.3")
gen.add("restore_defaults", bool_t, 0, "Retore to the default configuration", False)
exit(gen.generate(PACKAGE, "base_local_planner", "BaseLocalPlanner"))

10
navigations/base_local_planner/cfg/LocalPlannerLimits.cfg Executable file
View File

@@ -0,0 +1,10 @@
#!/usr/bin/env python
# Generic Local Planner configuration
# from dynamic_reconfigure.parameter_generator_catkin import *
# if __name__ == "__main__":
# gen = ParameterGenerator()
# add_generic_localplanner_params(gen)
# exit(gen.generate(PACKAGE, "base_local_planner", "LocalPlannerLimits"))

102
navigations/base_local_planner/include/base_local_planner/costmap_model.h Executable file
View File

@@ -0,0 +1,102 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#define TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#include <base_local_planner/world_model.h>
// For obstacle data access
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
/**
* @class CostmapModel
* @brief A class that implements the WorldModel interface to provide grid
* based collision checks for the trajectory controller using the costmap.
*/
class CostmapModel : public WorldModel {
public:
/**
* @brief Constructor for the CostmapModel
* @param costmap The costmap that should be used
* @return
*/
CostmapModel(const costmap_2d::Costmap2D& costmap);
/**
* @brief Destructor for the world model
*/
virtual ~CostmapModel(){}
using WorldModel::footprintCost;
/**
* @brief Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise:
* -1 if footprint covers at least a lethal obstacle cell, or
* -2 if footprint covers at least a no-information cell, or
* -3 if footprint is [partially] outside of the map
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
* @param x0 The x position of the first cell in grid coordinates
* @param y0 The y position of the first cell in grid coordinates
* @param x1 The x position of the second cell in grid coordinates
* @param y1 The y position of the second cell in grid coordinates
* @return A positive cost for a legal line... negative otherwise
*/
double lineCost(int x0, int x1, int y0, int y1) const;
/**
* @brief Checks the cost of a point in the costmap
* @param x The x position of the point in cell coordinates
* @param y The y position of the point in cell coordinates
* @return A positive cost for a legal point... negative otherwise
*/
double pointCost(int x, int y) const;
private:
const costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
};
};
#endif

87
navigations/base_local_planner/include/base_local_planner/footprint_helper.h Executable file
View File

@@ -0,0 +1,87 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef FOOTPRINT_HELPER_H_
#define FOOTPRINT_HELPER_H_
#include <vector>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/Point.h>
#include <Eigen/Core>
#include <base_local_planner/Position2DInt.h>
namespace base_local_planner {
class FootprintHelper {
public:
FootprintHelper();
virtual ~FootprintHelper();
/**
* @brief Used to get the cells that make up the footprint of the robot
* @param x_i The x position of the robot
* @param y_i The y position of the robot
* @param theta_i The orientation of the robot
* @param fill If true: returns all cells in the footprint of the robot. If false: returns only the cells that make up the outline of the footprint.
* @return The cells that make up either the outline or entire footprint of the robot depending on fill
*/
std::vector<base_local_planner::Position2DInt> getFootprintCells(
Eigen::Vector3f pos,
std::vector<geometry_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D&,
bool fill);
/**
* @brief Use Bresenham's algorithm to trace a line between two points in a grid
* @param x0 The x coordinate of the first point
* @param x1 The x coordinate of the second point
* @param y0 The y coordinate of the first point
* @param y1 The y coordinate of the second point
* @param pts Will be filled with the cells that lie on the line in the grid
*/
void getLineCells(int x0, int x1, int y0, int y1, std::vector<base_local_planner::Position2DInt>& pts);
/**
* @brief Fill the outline of a polygon, in this case the robot footprint, in a grid
* @param footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint
*/
void getFillCells(std::vector<base_local_planner::Position2DInt>& footprint);
};
} /* namespace base_local_planner */
#endif /* FOOTPRINT_HELPER_H_ */

153
navigations/base_local_planner/include/base_local_planner/goal_functions.h Executable file
View File

@@ -0,0 +1,153 @@
/*********************************************************************
*
* 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
*********************************************************************/
#ifndef BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#define BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf2_ros/buffer.h>
#include <string>
#include <cmath>
#include <angles/angles.h>
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
/**
* @brief return squared distance to check if the goal position has been achieved
* @param global_pose The pose of the robot in the global frame
* @param goal_x The desired x value for the goal
* @param goal_y The desired y value for the goal
* @return distance to goal
*/
double getGoalPositionDistance(const geometry_msgs::PoseStamped& global_pose, double goal_x, double goal_y);
/**
* @brief return angle difference to goal to check if the goal orientation has been achieved
* @param global_pose The pose of the robot in the global frame
* @param goal_x The desired x value for the goal
* @param goal_y The desired y value for the goal
* @return angular difference
*/
double getGoalOrientationAngleDifference(const geometry_msgs::PoseStamped& global_pose, double goal_th);
/**
* @brief Publish a plan for visualization purposes
* @param path The plan to publish
* @param pub The published to use
* @param r,g,b,a The color and alpha value to use when publishing
*/
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub);
/**
* @brief Trim off parts of the global plan that are far enough behind the robot
* @param global_pose The pose of the robot in the global frame
* @param plan The plan to be pruned
* @param global_plan The plan to be pruned in the frame of the planner
*/
void prunePlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Transforms the global plan of the robot from the planner frame to the frame of the costmap,
* selects only the (first) part of the plan that is within the costmap area.
* @param tf A reference to a transform listener
* @param global_plan The plan to be transformed
* @param robot_pose The pose of the robot in the global frame (same as costmap)
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
* @param global_frame The frame to transform the plan to
* @param transformed_plan Populated with the transformed plan
*/
bool transformGlobalPlan(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_robot_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan);
/**
* @brief Returns last pose in plan
* @param tf A reference to a transform listener
* @param global_plan The plan being followed
* @param global_frame The global frame of the local planner
* @param goal_pose the pose to copy into
* @return True if achieved, false otherwise
*/
bool getGoalPose(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame,
geometry_msgs::PoseStamped &goal_pose);
/**
* @brief Check if the goal pose has been achieved
* @param tf A reference to a transform listener
* @param global_plan The plan being followed
* @param costmap_ros A reference to the costmap object being used by the planner
* @param global_frame The global frame of the local planner
* @param base_odom The current odometry information for the robot
* @param rot_stopped_vel The rotational velocity below which the robot is considered stopped
* @param trans_stopped_vel The translational velocity below which the robot is considered stopped
* @param xy_goal_tolerance The translational tolerance on reaching the goal
* @param yaw_goal_tolerance The rotational tolerance on reaching the goal
* @return True if achieved, false otherwise
*/
bool isGoalReached(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
geometry_msgs::PoseStamped& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance);
/**
* @brief Check whether the robot is stopped or not
* @param base_odom The current odometry information for the robot
* @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped
* @param trans_stopped_velocity The translational velocity below which the robot is considered stopped
* @return True if the robot is stopped, false otherwise
*/
bool stopped(const nav_msgs::Odometry& base_odom,
const double& rot_stopped_velocity,
const double& trans_stopped_velocity);
};
#endif

93
navigations/base_local_planner/include/base_local_planner/latched_stop_rotate_controller.h Executable file
View File

@@ -0,0 +1,93 @@
/*
* latched_stop_rotate_controller.h
*
* Created on: Apr 16, 2012
* Author: tkruse
*/
#ifndef LATCHED_STOP_ROTATE_CONTROLLER_H_
#define LATCHED_STOP_ROTATE_CONTROLLER_H_
#include <string>
#include <Eigen/Core>
#include <base_local_planner/local_planner_util.h>
#include <base_local_planner/odometry_helper_ros.h>
namespace base_local_planner {
class LatchedStopRotateController {
public:
LatchedStopRotateController(const std::string& name = "");
virtual ~LatchedStopRotateController();
bool isPositionReached(LocalPlannerUtil* planner_util,
const geometry_msgs::PoseStamped& global_pose);
bool isGoalReached(LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper,
const geometry_msgs::PoseStamped& global_pose);
void resetLatching() {
xy_tolerance_latch_ = false;
}
/**
* @brief Stop the robot taking into account acceleration limits
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check);
/**
* @brief Once a goal position is reached... rotate to the goal orientation
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param goal_th The desired th value for the goal
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool rotateToGoal(const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
double goal_th,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
base_local_planner::LocalPlannerLimits& limits,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check);
bool computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper,
const geometry_msgs::PoseStamped& global_pose,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check);
private:
inline double sign(double x){
return x < 0.0 ? -1.0 : 1.0;
}
// whether to latch at all, and whether in this turn we have already been in goal area
bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
bool rotating_to_goal_;
};
} /* namespace base_local_planner */
#endif /* LATCHED_STOP_ROTATE_CONTROLLER_H_ */

143
navigations/base_local_planner/include/base_local_planner/line_iterator.h Executable file
View File

@@ -0,0 +1,143 @@
/*
* 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.
*/
#ifndef LINE_ITERATOR_H
#define LINE_ITERATOR_H
#include <stdlib.h>
namespace base_local_planner
{
/** An iterator implementing Bresenham Ray-Tracing. */
class LineIterator
{
public:
LineIterator( int x0, int y0, int x1, int y1 )
: x0_( x0 )
, y0_( y0 )
, x1_( x1 )
, y1_( y1 )
, x_( x0 ) // X and Y start of at first endpoint.
, y_( y0 )
, deltax_( abs( x1 - x0 ))
, deltay_( abs( y1 - y0 ))
, curpixel_( 0 )
{
if( x1_ >= x0_ ) // The x-values are increasing
{
xinc1_ = 1;
xinc2_ = 1;
}
else // The x-values are decreasing
{
xinc1_ = -1;
xinc2_ = -1;
}
if( y1_ >= y0_) // The y-values are increasing
{
yinc1_ = 1;
yinc2_ = 1;
}
else // The y-values are decreasing
{
yinc1_ = -1;
yinc2_ = -1;
}
if( deltax_ >= deltay_ ) // There is at least one x-value for every y-value
{
xinc1_ = 0; // Don't change the x when numerator >= denominator
yinc2_ = 0; // Don't change the y for every iteration
den_ = deltax_;
num_ = deltax_ / 2;
numadd_ = deltay_;
numpixels_ = deltax_; // There are more x-values than y-values
}
else // There is at least one y-value for every x-value
{
xinc2_ = 0; // Don't change the x for every iteration
yinc1_ = 0; // Don't change the y when numerator >= denominator
den_ = deltay_;
num_ = deltay_ / 2;
numadd_ = deltax_;
numpixels_ = deltay_; // There are more y-values than x-values
}
}
bool isValid() const
{
return curpixel_ <= numpixels_;
}
void advance()
{
num_ += numadd_; // Increase the numerator by the top of the fraction
if( num_ >= den_ ) // Check if numerator >= denominator
{
num_ -= den_; // Calculate the new numerator value
x_ += xinc1_; // Change the x as appropriate
y_ += yinc1_; // Change the y as appropriate
}
x_ += xinc2_; // Change the x as appropriate
y_ += yinc2_; // Change the y as appropriate
curpixel_++;
}
int getX() const { return x_; }
int getY() const { return y_; }
int getX0() const { return x0_; }
int getY0() const { return y0_; }
int getX1() const { return x1_; }
int getY1() const { return y1_; }
private:
int x0_; ///< X coordinate of first end point.
int y0_; ///< Y coordinate of first end point.
int x1_; ///< X coordinate of second end point.
int y1_; ///< Y coordinate of second end point.
int x_; ///< X coordinate of current point.
int y_; ///< Y coordinate of current point.
int deltax_; ///< Difference between Xs of endpoints.
int deltay_; ///< Difference between Ys of endpoints.
int curpixel_; ///< index of current point in line loop.
int xinc1_, xinc2_, yinc1_, yinc2_;
int den_, num_, numadd_, numpixels_;
};
} // end namespace base_local_planner
#endif // LINE_ITERATOR_H

121
navigations/base_local_planner/include/base_local_planner/local_planner_limits.h Executable file
View File

@@ -0,0 +1,121 @@
/***********************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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.
***********************************************************/
#ifndef __base_local_planner__LOCALPLANNERLIMITS_H__
#define __base_local_planner__LOCALPLANNERLIMITS_H__
#include <Eigen/Core>
namespace base_local_planner
{
class LocalPlannerLimits
{
public:
double max_vel_trans;
double min_vel_trans;
double max_vel_x;
double min_vel_x;
double max_vel_y;
double min_vel_y;
double max_vel_theta;
double min_vel_theta;
double acc_lim_x;
double acc_lim_y;
double acc_lim_theta;
double acc_lim_trans;
bool prune_plan;
double xy_goal_tolerance;
double yaw_goal_tolerance;
double trans_stopped_vel;
double theta_stopped_vel;
bool restore_defaults;
LocalPlannerLimits() {}
LocalPlannerLimits(
double nmax_vel_trans,
double nmin_vel_trans,
double nmax_vel_x,
double nmin_vel_x,
double nmax_vel_y,
double nmin_vel_y,
double nmax_vel_theta,
double nmin_vel_theta,
double nacc_lim_x,
double nacc_lim_y,
double nacc_lim_theta,
double nacc_lim_trans,
double nxy_goal_tolerance,
double nyaw_goal_tolerance,
bool nprune_plan = true,
double ntrans_stopped_vel = 0.1,
double ntheta_stopped_vel = 0.1):
max_vel_trans(nmax_vel_trans),
min_vel_trans(nmin_vel_trans),
max_vel_x(nmax_vel_x),
min_vel_x(nmin_vel_x),
max_vel_y(nmax_vel_y),
min_vel_y(nmin_vel_y),
max_vel_theta(nmax_vel_theta),
min_vel_theta(nmin_vel_theta),
acc_lim_x(nacc_lim_x),
acc_lim_y(nacc_lim_y),
acc_lim_theta(nacc_lim_theta),
acc_lim_trans(nacc_lim_trans),
prune_plan(nprune_plan),
xy_goal_tolerance(nxy_goal_tolerance),
yaw_goal_tolerance(nyaw_goal_tolerance),
trans_stopped_vel(ntrans_stopped_vel),
theta_stopped_vel(ntheta_stopped_vel) {}
~LocalPlannerLimits() {}
/**
* @brief Get the acceleration limits of the robot
* @return The acceleration limits of the robot
*/
Eigen::Vector3f getAccLimits() {
Eigen::Vector3f acc_limits;
acc_limits[0] = acc_lim_x;
acc_limits[1] = acc_lim_y;
acc_limits[2] = acc_lim_theta;
return acc_limits;
}
};
}
#endif // __LOCALPLANNERLIMITS_H__

111
navigations/base_local_planner/include/base_local_planner/local_planner_util.h Executable file
View File

@@ -0,0 +1,111 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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 ABSTRACT_LOCAL_PLANNER_ODOM_H_
#define ABSTRACT_LOCAL_PLANNER_ODOM_H_
#include <nav_core/base_local_planner.h>
#include <boost/thread.hpp>
#include <costmap_2d/costmap_2d.h>
#include <tf2_ros/buffer.h>
#include <base_local_planner/local_planner_limits.h>
namespace base_local_planner {
/**
* @class LocalPlannerUtil
* @brief Helper class implementing infrastructure code many local planner implementations may need.
*/
class LocalPlannerUtil {
private:
// things we get from move_base
std::string name_;
std::string global_frame_;
costmap_2d::Costmap2D* costmap_;
tf2_ros::Buffer* tf_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
boost::mutex limits_configuration_mutex_;
bool setup_;
LocalPlannerLimits default_limits_;
LocalPlannerLimits limits_;
bool initialized_;
public:
/**
* @brief Callback to update the local planner's parameters
*/
void reconfigureCB(LocalPlannerLimits &config, bool restore_defaults);
LocalPlannerUtil() : initialized_(false) {}
~LocalPlannerUtil() {
}
void initialize(tf2_ros::Buffer* tf,
costmap_2d::Costmap2D* costmap,
std::string global_frame);
bool getGoal(geometry_msgs::PoseStamped& goal_pose);
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
bool getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan);
costmap_2d::Costmap2D* getCostmap();
LocalPlannerLimits getCurrentLimits();
std::string getGlobalFrame(){ return global_frame_; }
};
};
#endif /* ABSTRACT_LOCAL_PLANNER_ODOM_H_ */

67
navigations/base_local_planner/include/base_local_planner/map_cell.h Executable file
View File

@@ -0,0 +1,67 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_MAP_CELL_H_
#define TRAJECTORY_ROLLOUT_MAP_CELL_H_
#include <base_local_planner/trajectory_inc.h>
namespace base_local_planner {
/**
* @class MapCell
* @brief Stores path distance and goal distance information used for scoring trajectories
*/
class MapCell{
public:
/**
* @brief Default constructor
*/
MapCell();
/**
* @brief Copy constructor
* @param mc The MapCell to be copied
*/
MapCell(const MapCell& mc);
unsigned int cx, cy; ///< @brief Cell index in the grid map
double target_dist; ///< @brief Distance to planner's path
bool target_mark; ///< @brief Marks for computing path/goal distances
bool within_robot; ///< @brief Mark for cells within the robot footprint
};
};
#endif

200
navigations/base_local_planner/include/base_local_planner/map_grid.h Executable file
View File

@@ -0,0 +1,200 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_
#define TRAJECTORY_ROLLOUT_MAP_GRID_H_
#include <vector>
#include <iostream>
#include <base_local_planner/trajectory_inc.h>
#include <ros/console.h>
#include <ros/ros.h>
#include <base_local_planner/map_cell.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
namespace base_local_planner{
/**
* @class MapGrid
* @brief A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controller.
*/
class MapGrid{
public:
/**
* @brief Creates a 0x0 map by default
*/
MapGrid();
/**
* @brief Creates a map of size_x by size_y
* @param size_x The width of the map
* @param size_y The height of the map
*/
MapGrid(unsigned int size_x, unsigned int size_y);
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A reference to the desired cell
*/
inline MapCell& operator() (unsigned int x, unsigned int y){
return map_[size_x_ * y + x];
}
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A copy of the desired cell
*/
inline MapCell operator() (unsigned int x, unsigned int y) const {
return map_[size_x_ * y + x];
}
inline MapCell& getCell(unsigned int x, unsigned int y){
return map_[size_x_ * y + x];
}
/**
* @brief Destructor for a MapGrid
*/
~MapGrid(){}
/**
* @brief Copy constructor for a MapGrid
* @param mg The MapGrid to copy
*/
MapGrid(const MapGrid& mg);
/**
* @brief Assignment operator for a MapGrid
* @param mg The MapGrid to assign from
*/
MapGrid& operator= (const MapGrid& mg);
/**
* @brief reset path distance fields for all cells
*/
void resetPathDist();
/**
* @brief check if we need to resize
* @param size_x The desired width
* @param size_y The desired height
*/
void sizeCheck(unsigned int size_x, unsigned int size_y);
/**
* @brief Utility to share initialization code across constructors
*/
void commonInit();
/**
* @brief Returns a 1D index into the MapCell array for a 2D index
* @param x The desired x coordinate
* @param y The desired y coordinate
* @return The associated 1D index
*/
size_t getIndex(int x, int y);
/**
* return a value that indicates cell is in obstacle
*/
inline double obstacleCosts() {
return map_.size();
}
/**
* returns a value indicating cell was not reached by wavefront
* propagation of set cells. (is behind walls, regarding the region covered by grid)
*/
inline double unreachableCellCosts() {
return map_.size() + 1;
}
/**
* @brief Used to update the distance of a cell in path distance computation
* @param current_cell The cell we're currently in
* @param check_cell The cell to be updated
*/
inline bool updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap);
/**
* increase global plan resolution to match that of the costmap by adding points linearly between global plan points
* This is necessary where global planners produce plans with few points.
* @param global_plan_in input
* @param global_plan_output output
* @param resolution desired distance between waypoints
*/
static void adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution);
/**
* @brief Compute the distance from each cell in the local map grid to the planned path
* @param dist_queue A queue of the initial cells on the path
*/
void computeTargetDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Compute the distance from each cell in the local map grid to the local goal point
* @param goal_queue A queue containing the local goal cell
*/
void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Update what cells are considered path based on the global plan
*/
void setTargetCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Update what cell is considered the next local goal
*/
void setLocalGoal(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan);
double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
private:
std::vector<MapCell> map_; ///< @brief Storage for the MapCells
};
};
#endif

139
navigations/base_local_planner/include/base_local_planner/map_grid_cost_function.h Executable file
View File

@@ -0,0 +1,139 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef MAP_GRID_COST_FUNCTION_H_
#define MAP_GRID_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
#include <costmap_2d/costmap_2d.h>
#include <base_local_planner/map_grid.h>
namespace base_local_planner {
/**
* when scoring a trajectory according to the values in mapgrid, we can take
*return the value of the last point (if no of the earlier points were in
* return collision), the sum for all points, or the product of all (non-zero) points
*/
enum CostAggregationType { Last, Sum, Product};
/**
* This class provides cost based on a map_grid of a small area of the world.
* The map_grid covers a the costmap, the costmap containing the information
* about sensed obstacles. The map_grid is used by setting
* certain cells to distance 0, and then propagating distances around them,
* filling up the area reachable around them.
*
* The approach using grid_maps is used for computational efficiency, allowing to
* score hundreds of trajectories very quickly.
*
* This can be used to favor trajectories which stay on a given path, or which
* approach a given goal.
* @param costmap_ros Reference to object giving updates of obstacles around robot
* @param xshift where the scoring point is with respect to robot center pose
* @param yshift where the scoring point is with respect to robot center pose
* @param is_local_goal_function, scores for local goal rather than whole path
* @param aggregationType how to combine costs along trajectory
*/
class MapGridCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
MapGridCostFunction(costmap_2d::Costmap2D* costmap,
double xshift = 0.0,
double yshift = 0.0,
bool is_local_goal_function = false,
CostAggregationType aggregationType = Last);
~MapGridCostFunction() {}
/**
* set line segments on the grid with distance 0, resets the grid
*/
void setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses);
void setXShift(double xshift) {xshift_ = xshift;}
void setYShift(double yshift) {yshift_ = yshift;}
/** @brief If true, failures along the path cause the entire path to be rejected.
*
* Default is true. */
void setStopOnFailure(bool stop_on_failure) {stop_on_failure_ = stop_on_failure;}
/**
* propagate distances
*/
bool prepare();
double scoreTrajectory(Trajectory &traj);
/**
* return a value that indicates cell is in obstacle
*/
double obstacleCosts() {
return map_.obstacleCosts();
}
/**
* returns a value indicating cell was not reached by wavefront
* propagation of set cells. (is behind walls, regarding the region covered by grid)
*/
double unreachableCellCosts() {
return map_.unreachableCellCosts();
}
// used for easier debugging
double getCellCosts(unsigned int cx, unsigned int cy);
private:
std::vector<geometry_msgs::PoseStamped> target_poses_;
costmap_2d::Costmap2D* costmap_;
base_local_planner::MapGrid map_;
CostAggregationType aggregationType_;
/// xshift and yshift allow scoring for different
// ooints of robots than center, like fron or back
// this can help with alignment or keeping specific
// wheels on tracks both default to 0
double xshift_;
double yshift_;
// if true, we look for a suitable local goal on path, else we use the full path for costs
bool is_local_goal_function_;
bool stop_on_failure_;
};
} /* namespace base_local_planner */
#endif /* MAP_GRID_COST_FUNCTION_H_ */

71
navigations/base_local_planner/include/base_local_planner/map_grid_visualizer.h Executable file
View File

@@ -0,0 +1,71 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Eric Perko
* 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 Eric Perko 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.
*********************************************************************/
#ifndef MAP_GRID_VISUALIZER_H_
#define MAP_GRID_VISUALIZER_H_
#include <ros/ros.h>
#include <base_local_planner/map_grid.h>
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
class MapGridVisualizer {
public:
/**
* @brief Default constructor
*/
MapGridVisualizer();
/**
* @brief Initializes the MapGridVisualizer
* @param name The name to be appended to ~/ in order to get the proper namespace for parameters
* @param costmap The costmap instance to use to get the size of the map to generate a point cloud for
* @param cost_function The function to use to compute the cost values to be inserted into each point in the output PointCloud as well as whether to include a given point or not
*/
void initialize(const std::string& name, std::string frame, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function);
/**
* @brief Build and publish a PointCloud if the publish_cost_grid_pc parameter was true. Only include points for which the cost_function at (cx,cy) returns true.
*/
void publishCostCloud(const costmap_2d::Costmap2D* costmap_p_);
private:
std::string name_; ///< @brief The name to get parameters relative to.
boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function_; ///< @brief The function to be used to generate the cost components for the output PointCloud
ros::NodeHandle ns_nh_;
std::string frame_id_;
ros::Publisher pub_;
};
};
#endif

89
navigations/base_local_planner/include/base_local_planner/obstacle_cost_function.h Executable file
View File

@@ -0,0 +1,89 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef OBSTACLE_COST_FUNCTION_H_
#define OBSTACLE_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/costmap_2d.h>
namespace base_local_planner {
/**
* class ObstacleCostFunction
* @brief Uses costmap 2d to assign negative costs if robot footprint
* is in obstacle on any point of the trajectory.
*/
class ObstacleCostFunction : public TrajectoryCostFunction {
public:
ObstacleCostFunction(costmap_2d::Costmap2D* costmap);
~ObstacleCostFunction();
bool prepare();
double scoreTrajectory(Trajectory &traj);
void setSumScores(bool score_sums){ sum_scores_=score_sums; }
void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed);
void setFootprint(std::vector<geometry_msgs::Point> footprint_spec);
// helper functions, made static for easy unit testing
static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor);
static double footprintCost(
const double& x,
const double& y,
const double& th,
double scale,
std::vector<geometry_msgs::Point> footprint_spec,
costmap_2d::Costmap2D* costmap,
base_local_planner::WorldModel* world_model);
private:
costmap_2d::Costmap2D* costmap_;
std::vector<geometry_msgs::Point> footprint_spec_;
base_local_planner::WorldModel* world_model_;
double max_trans_vel_;
bool sum_scores_;
//footprint scaling with velocity;
double max_scaling_factor_, scaling_speed_;
};
} /* namespace base_local_planner */
#endif /* OBSTACLE_COST_FUNCTION_H_ */

92
navigations/base_local_planner/include/base_local_planner/odometry_helper_ros.h Executable file
View File

@@ -0,0 +1,92 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef ODOMETRY_HELPER_ROS2_H_
#define ODOMETRY_HELPER_ROS2_H_
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <geometry_msgs/PoseStamped.h>
namespace base_local_planner {
class OdometryHelperRos {
public:
/** @brief Constructor.
* @param odom_topic The topic on which to subscribe to Odometry
* messages. If the empty string is given (the default), no
* subscription is done. */
OdometryHelperRos(std::string odom_topic = "");
~OdometryHelperRos() {}
/**
* @brief Callback for receiving odometry data
* @param msg An Odometry message
*/
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
void getOdom(nav_msgs::Odometry& base_odom);
void getRobotVel(geometry_msgs::PoseStamped& robot_vel);
/** @brief Set the odometry topic. This overrides what was set in the constructor, if anything.
*
* This unsubscribes from the old topic (if any) and subscribes to the new one (if any).
*
* If odom_topic is the empty string, this just unsubscribes from the previous topic. */
void setOdomTopic(std::string odom_topic);
/** @brief Return the current odometry topic. */
std::string getOdomTopic() const { return odom_topic_; }
private:
//odom topic
std::string odom_topic_;
// we listen on odometry on the odom topic
ros::Subscriber odom_sub_;
nav_msgs::Odometry base_odom_;
boost::mutex odom_mutex_;
// global tf frame id
std::string frame_id_; ///< The frame_id associated this data
};
} /* namespace base_local_planner */
#define CHUNKY 1
#endif /* ODOMETRY_HELPER_ROS2_H_ */

89
navigations/base_local_planner/include/base_local_planner/oscillation_cost_function.h Executable file
View File

@@ -0,0 +1,89 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef OSCILLATION_COST_FUNCTION_H_
#define OSCILLATION_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
#include <Eigen/Core>
namespace base_local_planner {
class OscillationCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
OscillationCostFunction();
virtual ~OscillationCostFunction();
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
/**
* @brief Reset the oscillation flags for the local planner
*/
void resetOscillationFlags();
void updateOscillationFlags(Eigen::Vector3f pos, base_local_planner::Trajectory* traj, double min_vel_trans);
void setOscillationResetDist(double dist, double angle);
private:
void resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev);
/**
* @brief Given a trajectory that's selected, set flags if needed to
* prevent the robot from oscillating
* @param t The selected trajectory
* @return True if a flag was set, false otherwise
*/
bool setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans);
// flags
bool strafe_pos_only_, strafe_neg_only_, strafing_pos_, strafing_neg_;
bool rot_pos_only_, rot_neg_only_, rotating_pos_, rotating_neg_;
bool forward_pos_only_, forward_neg_only_, forward_pos_, forward_neg_;
// param
double oscillation_reset_dist_, oscillation_reset_angle_;
Eigen::Vector3f prev_stationary_pos_;
};
} /* namespace base_local_planner */
#endif /* OSCILLATION_COST_FUNCTION_H_ */

57
navigations/base_local_planner/include/base_local_planner/planar_laser_scan.h Executable file
View File

@@ -0,0 +1,57 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#define TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#include <geometry_msgs/Point32.h>
#include <sensor_msgs/PointCloud.h>
namespace base_local_planner {
/**
* @class PlanarLaserScan
* @brief Stores a scan from a planar laser that can be used to clear freespace
*/
class PlanarLaserScan {
public:
PlanarLaserScan() {}
geometry_msgs::Point32 origin;
sensor_msgs::PointCloud cloud;
double angle_min, angle_max, angle_increment;
};
};
#endif

326
navigations/base_local_planner/include/base_local_planner/point_grid.h Executable file
View File

@@ -0,0 +1,326 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 POINT_GRID_H_
#define POINT_GRID_H_
#include <vector>
#include <list>
#include <cfloat>
#include <geometry_msgs/Point.h>
#include <costmap_2d/observation.h>
#include <base_local_planner/world_model.h>
#include <sensor_msgs/PointCloud2.h>
namespace base_local_planner {
/**
* @class PointGrid
* @brief A class that implements the WorldModel interface to provide
* free-space collision checks for the trajectory controller. This class
* stores points binned into a grid and performs point-in-polygon checks when
* necessary to determine the legality of a footprint at a given
* position/orientation.
*/
class PointGrid : public WorldModel {
public:
/**
* @brief Constuctor for a grid that stores points in the plane
* @param width The width in meters of the grid
* @param height The height in meters of the gird
* @param resolution The resolution of the grid in meters/cell
* @param origin The origin of the bottom left corner of the grid
* @param max_z The maximum height for an obstacle to be added to the grid
* @param obstacle_range The maximum distance for obstacles to be added to the grid
* @param min_separation The minimum distance between points in the grid
*/
PointGrid(double width, double height, double resolution, geometry_msgs::Point origin,
double max_z, double obstacle_range, double min_separation);
/**
* @brief Destructor for a point grid
*/
virtual ~PointGrid(){}
/**
* @brief Returns the points that lie within the cells contained in the specified range. Some of these points may be outside the range itself.
* @param lower_left The lower left corner of the range search
* @param upper_right The upper right corner of the range search
* @param points A vector of pointers to lists of the relevant points
*/
void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<geometry_msgs::Point32>* >& points);
/**
* @brief Checks if any points in the grid lie inside a convex footprint
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
using WorldModel::footprintCost;
/**
* @brief Inserts observations from sensors into the point grid
* @param footprint The footprint of the robot in its current location
* @param observations The observations from various sensors
* @param laser_scans The laser scans used to clear freespace (the point grid only uses the first scan which is assumed to be the base laser)
*/
void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
* @brief Convert from world coordinates to grid coordinates
* @param pt A point in world space
* @param gx The x coordinate of the corresponding grid cell to be set by the function
* @param gy The y coordinate of the corresponding grid cell to be set by the function
* @return True if the conversion was successful, false otherwise
*/
inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
if(pt.x < origin_.x || pt.y < origin_.y){
gx = 0;
gy = 0;
return false;
}
gx = (int) ((pt.x - origin_.x)/resolution_);
gy = (int) ((pt.y - origin_.y)/resolution_);
if(gx >= width_ || gy >= height_){
gx = 0;
gy = 0;
return false;
}
return true;
}
/**
* @brief Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called
* @param gx The x coordinate of the grid cell
* @param gy The y coordinate of the grid cell
* @param lower_left The lower left bounds of the cell in world coordinates to be filled in
* @param upper_right The upper right bounds of the cell in world coordinates to be filled in
*/
inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
lower_left.x = gx * resolution_ + origin_.x;
lower_left.y = gy * resolution_ + origin_.y;
upper_right.x = lower_left.x + resolution_;
upper_right.y = lower_left.y + resolution_;
}
/**
* @brief Compute the squared distance between two points
* @param pt1 The first point
* @param pt2 The second point
* @return The squared distance between the two points
*/
inline double sq_distance(const geometry_msgs::Point32& pt1, const geometry_msgs::Point32& pt2){
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
}
/**
* @brief Convert from world coordinates to grid coordinates
* @param pt A point in world space
* @param gx The x coordinate of the corresponding grid cell to be set by the function
* @param gy The y coordinate of the corresponding grid cell to be set by the function
* @return True if the conversion was successful, false otherwise
*/
inline bool gridCoords(const geometry_msgs::Point32& pt, unsigned int& gx, unsigned int& gy) const {
if(pt.x < origin_.x || pt.y < origin_.y){
gx = 0;
gy = 0;
return false;
}
gx = (int) ((pt.x - origin_.x)/resolution_);
gy = (int) ((pt.y - origin_.y)/resolution_);
if(gx >= width_ || gy >= height_){
gx = 0;
gy = 0;
return false;
}
return true;
}
/**
* @brief Converts cell coordinates to an index value that can be used to look up the correct grid cell
* @param gx The x coordinate of the cell
* @param gy The y coordinate of the cell
* @return The index of the cell in the stored cell list
*/
inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
/*
* (0, 0) ---------------------- (width, 0)
* | |
* | |
* | |
* | |
* | |
* (0, height) ----------------- (width, height)
*/
return(gx + gy * width_);
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const geometry_msgs::Point32& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
template<typename T>
inline double orient(const T& a, const T& b, const T& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check if two line segmenst intersect
* @param v1 The first point of the first segment
* @param v2 The second point of the first segment
* @param u1 The first point of the second segment
* @param u2 The second point of the second segment
* @return True if the segments intersect, false otherwise
*/
inline bool segIntersect(const geometry_msgs::Point32& v1, const geometry_msgs::Point32& v2,
const geometry_msgs::Point32& u1, const geometry_msgs::Point32& u2){
return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
}
/**
* @brief Find the intersection point of two lines
* @param v1 The first point of the first segment
* @param v2 The second point of the first segment
* @param u1 The first point of the second segment
* @param u2 The second point of the second segment
* @param result The point to be filled in
*/
void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
const geometry_msgs::Point& u1, const geometry_msgs::Point& u2,
geometry_msgs::Point& result);
/**
* @brief Check if a point is in a polygon
* @param pt The point to be checked
* @param poly The polygon to check against
* @return True if the point is in the polygon, false otherwise
*/
bool ptInPolygon(const geometry_msgs::Point32& pt, const std::vector<geometry_msgs::Point>& poly);
/**
* @brief Insert a point into the point grid
* @param pt The point to be inserted
*/
void insert(const geometry_msgs::Point32& pt);
/**
* @brief Find the distance between a point and its nearest neighbor in the grid
* @param pt The point used for comparison
* @return The distance between the point passed in and its nearest neighbor in the point grid
*/
double nearestNeighborDistance(const geometry_msgs::Point32& pt);
/**
* @brief Find the distance between a point and its nearest neighbor in a cell
* @param pt The point used for comparison
* @param gx The x coordinate of the cell
* @param gy The y coordinate of the cell
* @return The distance between the point passed in and its nearest neighbor in the cell
*/
double getNearestInCell(const geometry_msgs::Point32& pt, unsigned int gx, unsigned int gy);
/**
* @brief Removes points from the grid that lie within the polygon
* @param poly A specification of the polygon to clear from the grid
*/
void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);
/**
* @brief Removes points from the grid that lie within a laser scan
* @param laser_scan A specification of the laser scan to use for clearing
*/
void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
/**
* @brief Checks to see if a point is within a laser scan specification
* @param pt The point to check
* @param laser_scan The specification of the scan to check against
* @return True if the point is contained within the scan, false otherwise
*/
bool ptInScan(const geometry_msgs::Point32& pt, const PlanarLaserScan& laser_scan);
/**
* @brief Get the points in the point grid
* @param cloud The point cloud to insert the points into
*/
void getPoints(sensor_msgs::PointCloud2& cloud);
private:
double resolution_; ///< @brief The resolution of the grid in meters/cell
geometry_msgs::Point origin_; ///< @brief The origin point of the grid
unsigned int width_; ///< @brief The width of the grid in cells
unsigned int height_; ///< @brief The height of the grid in cells
std::vector< std::list<geometry_msgs::Point32> > cells_; ///< @brief Storage for the cells in the grid
double max_z_; ///< @brief The height cutoff for adding points as obstacles
double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
double sq_min_separation_; ///< @brief The minimum square distance required between points in the grid
std::vector< std::list<geometry_msgs::Point32>* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation
};
};
#endif

64
navigations/base_local_planner/include/base_local_planner/prefer_forward_cost_function.h Executable file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef PREFER_FORWARD_COST_FUNCTION_H_
#define PREFER_FORWARD_COST_FUNCTION_H_
#include <base_local_planner/trajectory_cost_function.h>
namespace base_local_planner {
class PreferForwardCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
PreferForwardCostFunction(double penalty) : penalty_(penalty) {}
~PreferForwardCostFunction() {}
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
void setPenalty(double penalty) {
penalty_ = penalty;
}
private:
double penalty_;
};
} /* namespace base_local_planner */
#endif /* PREFER_FORWARD_COST_FUNCTION_H_ */

109
navigations/base_local_planner/include/base_local_planner/simple_scored_sampling_planner.h Executable file
View File

@@ -0,0 +1,109 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef SIMPLE_SCORED_SAMPLING_PLANNER_H_
#define SIMPLE_SCORED_SAMPLING_PLANNER_H_
#include <vector>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/trajectory_cost_function.h>
#include <base_local_planner/trajectory_sample_generator.h>
#include <base_local_planner/trajectory_search.h>
namespace base_local_planner {
/**
* @class SimpleScoredSamplingPlanner
* @brief Generates a local plan using the given generator and cost functions.
* Assumes less cost are best, and negative costs indicate infinite costs
*
* This is supposed to be a simple and robust implementation of
* the TrajectorySearch interface. More efficient search may well be
* possible using search heuristics, parallel search, etc.
*/
class SimpleScoredSamplingPlanner : public base_local_planner::TrajectorySearch {
public:
~SimpleScoredSamplingPlanner() {}
SimpleScoredSamplingPlanner() {}
/**
* Takes a list of generators and critics. Critics return costs > 0, or negative costs for invalid trajectories.
* Generators other than the first are fallback generators, meaning they only get to generate if the previous
* generator did not find a valid trajectory.
* Will use every generator until it stops returning trajectories or count reaches max_samples.
* Then resets count and tries for the next in the list.
* passing max_samples = -1 (default): Each Sampling planner will continue to call
* generator until generator runs out of samples (or forever if that never happens)
*/
SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples = -1);
/**
* runs all scoring functions over the trajectory creating a weigthed sum
* of positive costs, aborting as soon as a negative cost are found or costs greater
* than positive best_traj_cost accumulated
*/
double scoreTrajectory(Trajectory& traj, double best_traj_cost);
/**
* Calls generator until generator has no more samples or max_samples is reached.
* For each generated traj, calls critics in turn. If any critic returns negative
* value, that value is assumed as costs, else the costs are the sum of all critics
* result. Returns true and sets the traj parameter to the first trajectory with
* minimal non-negative costs if sampling yields trajectories with non-negative costs,
* else returns false.
*
* @param traj The container to write the result to
* @param all_explored pass NULL or a container to collect all trajectories for debugging (has a penalty)
*/
bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored = 0);
private:
std::vector<TrajectorySampleGenerator*> gen_list_;
std::vector<TrajectoryCostFunction*> critics_;
int max_samples_;
};
} // namespace
#endif /* SIMPLE_SCORED_SAMPLING_PLANNER_H_ */

160
navigations/base_local_planner/include/base_local_planner/simple_trajectory_generator.h Executable file
View File

@@ -0,0 +1,160 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef SIMPLE_TRAJECTORY_GENERATOR_H_
#define SIMPLE_TRAJECTORY_GENERATOR_H_
#include <base_local_planner/trajectory_sample_generator.h>
#include <base_local_planner/local_planner_limits.h>
#include <Eigen/Core>
namespace base_local_planner {
/**
* generates trajectories based on equi-distant discretisation of the degrees of freedom.
* This is supposed to be a simple and robust implementation of the TrajectorySampleGenerator
* interface, more efficient implementations are thinkable.
*
* This can be used for both dwa and trajectory rollout approaches.
* As an example, assuming these values:
* sim_time = 1s, sim_period=200ms, dt = 200ms,
* vsamples_x=5,
* acc_limit_x = 1m/s^2, vel_x=0 (robot at rest, values just for easy calculations)
* dwa_planner will sample max-x-velocities from 0m/s to 0.2m/s.
* trajectory rollout approach will sample max-x-velocities 0m/s up to 1m/s
* trajectory rollout approach does so respecting the acceleration limit, so it gradually increases velocity
*/
class SimpleTrajectoryGenerator: public base_local_planner::TrajectorySampleGenerator {
public:
SimpleTrajectoryGenerator() {
limits_ = NULL;
}
~SimpleTrajectoryGenerator() {}
/**
* @param pos current robot position
* @param vel current robot velocity
* @param limits Current velocity limits
* @param vsamples: in how many samples to divide the given dimension
* @param use_acceleration_limits: if true use physical model, else idealized robot model
* @param additional_samples (deprecated): Additional velocity samples to generate individual trajectories from.
* @param discretize_by_time if true, the trajectory is split according in chunks of the same duration, else of same length
*/
void initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
std::vector<Eigen::Vector3f> additional_samples,
bool discretize_by_time = false);
/**
* @param pos current robot position
* @param vel current robot velocity
* @param limits Current velocity limits
* @param vsamples: in how many samples to divide the given dimension
* @param use_acceleration_limits: if true use physical model, else idealized robot model
* @param discretize_by_time if true, the trajectory is split according in chunks of the same duration, else of same length
*/
void initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time = false);
/**
* This function is to be called only when parameters change
*
* @param sim_granularity granularity of collision detection
* @param angular_sim_granularity angular granularity of collision detection
* @param use_dwa whether to use DWA or trajectory rollout
* @param sim_period distance between points in one trajectory
*/
void setParameters(double sim_time,
double sim_granularity,
double angular_sim_granularity,
bool use_dwa = false,
double sim_period = 0.0);
/**
* Whether this generator can create more trajectories
*/
bool hasMoreTrajectories();
/**
* Whether this generator can create more trajectories
*/
bool nextTrajectory(Trajectory &traj);
static Eigen::Vector3f computeNewPositions(const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel, double dt);
static Eigen::Vector3f computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt);
bool generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj);
protected:
unsigned int next_sample_index_;
// to store sample params of each sample between init and generation
std::vector<Eigen::Vector3f> sample_params_;
base_local_planner::LocalPlannerLimits* limits_;
Eigen::Vector3f pos_;
Eigen::Vector3f vel_;
// whether velocity of trajectory changes over time or not
bool continued_acceleration_;
bool discretize_by_time_;
double sim_time_, sim_granularity_, angular_sim_granularity_;
bool use_dwa_;
double sim_period_; // only for dwa
};
} /* namespace base_local_planner */
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */

118
navigations/base_local_planner/include/base_local_planner/trajectory.h Executable file
View File

@@ -0,0 +1,118 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#include <vector>
namespace base_local_planner {
/**
* @class Trajectory
* @brief Holds a trajectory generated by considering an x, y, and theta velocity
*/
class Trajectory {
public:
/**
* @brief Default constructor
*/
Trajectory();
/**
* @brief Constructs a trajectory
* @param xv The x velocity used to seed the trajectory
* @param yv The y velocity used to seed the trajectory
* @param thetav The theta velocity used to seed the trajectory
* @param num_pts The expected number of points for a trajectory
*/
Trajectory(double xv, double yv, double thetav, double time_delta, unsigned int num_pts);
double xv_, yv_, thetav_; ///< @brief The x, y, and theta velocities of the trajectory
double cost_; ///< @brief The cost/score of the trajectory
double time_delta_; ///< @brief The time gap between points
/**
* @brief Get a point within the trajectory
* @param index The index of the point to get
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getPoint(unsigned int index, double& x, double& y, double& th) const;
/**
* @brief Set a point within the trajectory
* @param index The index of the point to set
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void setPoint(unsigned int index, double x, double y, double th);
/**
* @brief Add a point to the end of a trajectory
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void addPoint(double x, double y, double th);
/**
* @brief Get the last point of the trajectory
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getEndpoint(double& x, double& y, double& th) const;
/**
* @brief Clear the trajectory's points
*/
void resetPoints();
/**
* @brief Return the number of points in the trajectory
* @return The number of points in the trajectory
*/
unsigned int getPointsSize() const;
private:
std::vector<double> x_pts_; ///< @brief The x points in the trajectory
std::vector<double> y_pts_; ///< @brief The y points in the trajectory
std::vector<double> th_pts_; ///< @brief The theta points in the trajectory
};
};
#endif

86
navigations/base_local_planner/include/base_local_planner/trajectory_cost_function.h Executable file
View File

@@ -0,0 +1,86 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef TRAJECTORYCOSTFUNCTION_H_
#define TRAJECTORYCOSTFUNCTION_H_
#include <base_local_planner/trajectory.h>
namespace base_local_planner {
/**
* @class TrajectoryCostFunction
* @brief Provides an interface for critics of trajectories
* During each sampling run, a batch of many trajectories will be scored using such a cost function.
* The prepare method is called before each batch run, and then for each
* trajectory of the sampling set, score_trajectory may be called.
*/
class TrajectoryCostFunction {
public:
/**
*
* General updating of context values if required.
* Subclasses may overwrite. Return false in case there is any error.
*/
virtual bool prepare() = 0;
/**
* return a score for trajectory traj
*/
virtual double scoreTrajectory(Trajectory &traj) = 0;
double getScale() {
return scale_;
}
void setScale(double scale) {
scale_ = scale;
}
virtual ~TrajectoryCostFunction() {}
protected:
TrajectoryCostFunction(double scale = 1.0): scale_(scale) {}
private:
double scale_;
};
}
#endif /* TRAJECTORYCOSTFUNCTION_H_ */

47
navigations/base_local_planner/include/base_local_planner/trajectory_inc.h Executable file
View File

@@ -0,0 +1,47 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_INC_H_
#define TRAJECTORY_INC_H_
#include <limits>
#ifndef DBL_MAX /* Max decimal value of a double */
#define DBL_MAX std::numeric_limits<double>::max() // 1.7976931348623157e+308
#endif
#ifndef DBL_MIN //Min decimal value of a double
#define DBL_MIN std::numeric_limits<double>::min() // 2.2250738585072014e-308
#endif
#endif

385
navigations/base_local_planner/include/base_local_planner/trajectory_planner.h Executable file
View File

@@ -0,0 +1,385 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#include <vector>
#include <cmath>
//for obstacle data access
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/cost_values.h>
#include <base_local_planner/footprint_helper.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/Position2DInt.h>
#include <base_local_planner/BaseLocalPlannerConfig.h>
//we'll take in a path as a vector of poses
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
//for creating a local cost grid
#include <base_local_planner/map_cell.h>
#include <base_local_planner/map_grid.h>
namespace base_local_planner {
/**
* @class TrajectoryPlanner
* @brief Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world.
*/
class TrajectoryPlanner{
friend class TrajectoryPlannerTest; //Need this for gtest to work
public:
/**
* @brief Constructs a trajectory controller
* @param world_model The WorldModel the trajectory controller uses to check for collisions
* @param costmap A reference to the Costmap the controller should use
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @param acc_lim_x The acceleration limit of the robot in the x direction
* @param acc_lim_y The acceleration limit of the robot in the y direction
* @param acc_lim_theta The acceleration limit of the robot in the theta direction
* @param sim_time The number of seconds to "roll-out" each trajectory
* @param sim_granularity The distance between simulation points should be small enough that the robot doesn't hit things
* @param vx_samples The number of trajectories to sample in the x dimension
* @param vtheta_samples The number of trajectories to sample in the theta dimension
* @param path_distance_bias A scaling factor for how close the robot should stay to the path
* @param goal_distance_bias A scaling factor for how aggresively the robot should pursue a local goal
* @param occdist_scale A scaling factor for how much the robot should prefer to stay away from obstacles
* @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities
* @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
* @param escape_reset_dist The distance the robot must travel before it can exit escape mode
* @param escape_reset_theta The distance the robot must rotate before it can exit escape mode
* @param holonomic_robot Set this to true if the robot being controlled can take y velocities and false otherwise
* @param max_vel_x The maximum x velocity the controller will explore
* @param min_vel_x The minimum x velocity the controller will explore
* @param max_vel_th The maximum rotational velocity the controller will explore
* @param min_vel_th The minimum rotational velocity the controller will explore
* @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore
* @param backup_vel The velocity to use while backing up
* @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits
* @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep
* @param heading_scoring_timestep How far to look ahead in time when we score heading based trajectories
* @param meter_scoring adapt parameters to costmap resolution
* @param simple_attractor Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation
* @param y_vels A vector of the y velocities the controller will explore
* @param angular_sim_granularity The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things
*/
TrajectoryPlanner(WorldModel& world_model,
const costmap_2d::Costmap2D& costmap,
std::vector<geometry_msgs::Point> footprint_spec,
double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
double sim_time = 1.0, double sim_granularity = 0.025,
int vx_samples = 20, int vtheta_samples = 20,
double path_distance_bias = 0.6, double goal_distance_bias = 0.8, double occdist_scale = 0.2,
double heading_lookahead = 0.325, double oscillation_reset_dist = 0.05,
double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2,
bool holonomic_robot = true,
double max_vel_x = 0.5, double min_vel_x = 0.1,
double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4,
double backup_vel = -0.1,
bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1,
bool meter_scoring = true,
bool simple_attractor = false,
std::vector<double> y_vels = std::vector<double>(0),
double stop_time_buffer = 0.2,
double sim_period = 0.1, double angular_sim_granularity = 0.025);
/**
* @brief Destructs a trajectory controller
*/
~TrajectoryPlanner();
/**
* @brief Reconfigures the trajectory planner
*/
void reconfigure(BaseLocalPlannerConfig &cfg);
/**
* @brief Given the current position, orientation, and velocity of the robot, return a trajectory to follow
* @param global_pose The current pose of the robot in world space
* @param global_vel The current velocity of the robot in world space
* @param drive_velocities Will be set to velocities to send to the robot base
* @return The selected path or trajectory
*/
Trajectory findBestPath(const geometry_msgs::PoseStamped& global_pose,
geometry_msgs::PoseStamped& global_vel, geometry_msgs::PoseStamped& drive_velocities);
/**
* @brief Update the plan that the controller is following
* @param new_plan A new plan for the controller to follow
* @param compute_dists Wheter or not to compute path/goal distances when a plan is updated
*/
void updatePlan(const std::vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists = false);
/**
* @brief Accessor for the goal the robot is currently pursuing in world corrdinates
* @param x Will be set to the x position of the local goal
* @param y Will be set to the y position of the local goal
*/
void getLocalGoal(double& x, double& y);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @return True if the trajectory is legal, false otherwise
*/
bool checkTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @return The score (as double)
*/
double scoreTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
/**
* @brief Compute the components and total cost for a map grid cell
* @param cx The x coordinate of the cell in the map grid
* @param cy The y coordinate of the cell in the map grid
* @param path_cost Will be set to the path distance component of the cost function
* @param goal_cost Will be set to the goal distance component of the cost function
* @param occ_cost Will be set to the costmap value of the cell
* @param total_cost Will be set to the value of the overall cost function, taking into account the scaling parameters
* @return True if the cell is traversible and therefore a legal location for the robot to move to
*/
bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
/** @brief Set the footprint specification of the robot. */
void setFootprint( std::vector<geometry_msgs::Point> footprint ) { footprint_spec_ = footprint; }
/** @brief Return the footprint specification of the robot. */
geometry_msgs::Polygon getFootprintPolygon() const { return costmap_2d::toPolygon(footprint_spec_); }
std::vector<geometry_msgs::Point> getFootprint() const { return footprint_spec_; }
private:
/**
* @brief Create the trajectories we wish to explore, score them, and return the best option
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param acc_x The x acceleration limit of the robot
* @param acc_y The y acceleration limit of the robot
* @param acc_theta The theta acceleration limit of the robot
* @return
*/
Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
double acc_x, double acc_y, double acc_theta);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param acc_x The x acceleration limit of the robot
* @param acc_y The y acceleration limit of the robot
* @param acc_theta The theta acceleration limit of the robot
* @param impossible_cost The cost value of a cell in the local map grid that is considered impassable
* @param traj Will be set to the generated trajectory with its associated score
*/
void generateTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,
double acc_theta, double impossible_cost, Trajectory& traj);
/**
* @brief Checks the legality of the robot footprint at a position and orientation using the world model
* @param x_i The x position of the robot
* @param y_i The y position of the robot
* @param theta_i The orientation of the robot
* @return
*/
double footprintCost(double x_i, double y_i, double theta_i);
base_local_planner::FootprintHelper footprint_helper_;
MapGrid path_map_; ///< @brief The local map grid where we propagate path distance
MapGrid goal_map_; ///< @brief The local map grid where we propagate goal distance
const costmap_2d::Costmap2D& costmap_; ///< @brief Provides access to cost map information
WorldModel& world_model_; ///< @brief The world model that the controller uses for collision detection
std::vector<geometry_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
std::vector<geometry_msgs::PoseStamped> global_plan_; ///< @brief The global path for the robot to follow
bool stuck_left, stuck_right; ///< @brief Booleans to keep the robot from oscillating during rotation
bool rotating_left, rotating_right; ///< @brief Booleans to keep track of the direction of rotation for the robot
bool stuck_left_strafe, stuck_right_strafe; ///< @brief Booleans to keep the robot from oscillating during strafing
bool strafe_right, strafe_left; ///< @brief Booleans to keep track of strafe direction for the robot
bool escaping_; ///< @brief Boolean to keep track of whether we're in escape mode
bool meter_scoring_;
double goal_x_,goal_y_; ///< @brief Storage for the local goal the robot is pursuing
double final_goal_x_, final_goal_y_; ///< @brief The end position of the plan.
bool final_goal_position_valid_; ///< @brief True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent.
double sim_time_; ///< @brief The number of seconds each trajectory is "rolled-out"
double sim_granularity_; ///< @brief The distance between simulation points
double angular_sim_granularity_; ///< @brief The distance between angular simulation points
int vx_samples_; ///< @brief The number of samples we'll take in the x dimenstion of the control space
int vtheta_samples_; ///< @brief The number of samples we'll take in the theta dimension of the control space
double path_distance_bias_, goal_distance_bias_, occdist_scale_; ///< @brief Scaling factors for the controller's cost function
double acc_lim_x_, acc_lim_y_, acc_lim_theta_; ///< @brief The acceleration limits of the robot
double prev_x_, prev_y_; ///< @brief Used to calculate the distance the robot has traveled before reseting oscillation booleans
double escape_x_, escape_y_, escape_theta_; ///< @brief Used to calculate the distance the robot has traveled before reseting escape booleans
Trajectory traj_one, traj_two; ///< @brief Used for scoring trajectories
double heading_lookahead_; ///< @brief How far the robot should look ahead of itself when differentiating between different rotational velocities
double oscillation_reset_dist_; ///< @brief The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
double escape_reset_dist_, escape_reset_theta_; ///< @brief The distance the robot must travel before it can leave escape mode
bool holonomic_robot_; ///< @brief Is the robot holonomic or not?
double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_vel_th_; ///< @brief Velocity limits for the controller
double backup_vel_; ///< @brief The velocity to use while backing up
bool dwa_; ///< @brief Should we use the dynamic window approach?
bool heading_scoring_; ///< @brief Should we score based on the rollout approach or the heading approach
double heading_scoring_timestep_; ///< @brief How far to look ahead in time when we score a heading
bool simple_attractor_; ///< @brief Enables simple attraction to a goal point
std::vector<double> y_vels_; ///< @brief Y velocities to explore
double stop_time_buffer_; ///< @brief How long before hitting something we're going to enforce that the robot stop
double sim_period_; ///< @brief The number of seconds to use to compute max/min vels for dwa
double inscribed_radius_, circumscribed_radius_;
boost::mutex configuration_mutex_;
/**
* @brief Compute x position based on velocity
* @param xi The current x position
* @param vx The current x velocity
* @param vy The current y velocity
* @param theta The current orientation
* @param dt The timestep to take
* @return The new x position
*/
inline double computeNewXPosition(double xi, double vx, double vy, double theta, double dt){
return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt;
}
/**
* @brief Compute y position based on velocity
* @param yi The current y position
* @param vx The current x velocity
* @param vy The current y velocity
* @param theta The current orientation
* @param dt The timestep to take
* @return The new y position
*/
inline double computeNewYPosition(double yi, double vx, double vy, double theta, double dt){
return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt;
}
/**
* @brief Compute orientation based on velocity
* @param thetai The current orientation
* @param vth The current theta velocity
* @param dt The timestep to take
* @return The new orientation
*/
inline double computeNewThetaPosition(double thetai, double vth, double dt){
return thetai + vth * dt;
}
//compute velocity based on acceleration
/**
* @brief Compute velocity based on acceleration
* @param vg The desired velocity, what we're accelerating up to
* @param vi The current velocity
* @param a_max An acceleration limit
* @param dt The timestep to take
* @return The new velocity
*/
inline double computeNewVelocity(double vg, double vi, double a_max, double dt){
if((vg - vi) >= 0) {
return std::min(vg, vi + a_max * dt);
}
return std::max(vg, vi - a_max * dt);
}
void getMaxSpeedToStopInTime(double time, double& vx, double& vy, double& vth){
vx = acc_lim_x_ * std::max(time, 0.0);
vy = acc_lim_y_ * std::max(time, 0.0);
vth = acc_lim_theta_ * std::max(time, 0.0);
}
double lineCost(int x0, int x1, int y0, int y1);
double pointCost(int x, int y);
double headingDiff(int cell_x, int cell_y, double x, double y, double heading);
};
};
#endif

231
navigations/base_local_planner/include/base_local_planner/trajectory_planner_ros.h Executable file
View File

@@ -0,0 +1,231 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/point_grid.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/voxel_grid_model.h>
#include <base_local_planner/trajectory_planner.h>
#include <base_local_planner/map_grid_visualizer.h>
#include <base_local_planner/planar_laser_scan.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf2_ros/buffer.h>
#include <boost/thread.hpp>
#include <string>
#include <angles/angles.h>
#include <nav_core/base_local_planner.h>
#include <dynamic_reconfigure/server.h>
#include <base_local_planner/BaseLocalPlannerConfig.h>
#include <base_local_planner/odometry_helper_ros.h>
namespace base_local_planner {
/**
* @class TrajectoryPlannerROS
* @brief A ROS wrapper for the trajectory controller that queries the param server to construct a controller
*/
class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner {
public:
/**
* @brief Default constructor for the ros wrapper
*/
TrajectoryPlannerROS();
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
TrajectoryPlannerROS(std::string name,
tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
void initialize(std::string name, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Destructor for the wrapper
*/
~TrajectoryPlannerROS();
/**
* @brief Given the current position, orientation, and velocity of the robot,
* compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
* @brief Set the plan that the controller is following
* @param orig_global_plan The plan to pass to the controller
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
* @brief Check if the goal pose has been achieved
* @return True if achieved, false otherwise
*/
bool isGoalReached();
/**
* @brief Generate and score a single trajectory
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param update_map Whether or not to update the map for the planner
* when computing the legality of the trajectory, this is useful to set
* to false if you're going to be doing a lot of trajectory checking over
* a short period of time
* @return True if the trajectory is legal, false otherwise
*/
bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
/**
* @brief Generate and score a single trajectory
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param update_map Whether or not to update the map for the planner
* when computing the legality of the trajectory, this is useful to set
* to false if you're going to be doing a lot of trajectory checking over
* a short period of time
* @return score of trajectory (double)
*/
double scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
bool isInitialized() {
return initialized_;
}
/** @brief Return the inner TrajectoryPlanner object. Only valid after initialize(). */
TrajectoryPlanner* getPlanner() const { return tc_; }
private:
/**
* @brief Callback to update the local planner's parameters based on dynamic reconfigure
*/
void reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level);
/**
* @brief Once a goal position is reached... rotate to the goal orientation
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param goal_th The desired th value for the goal
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool rotateToGoal(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel);
/**
* @brief Stop the robot taking into account acceleration limits
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, geometry_msgs::Twist& cmd_vel);
std::vector<double> loadYVels(ros::NodeHandle node);
double sign(double x){
return x < 0.0 ? -1.0 : 1.0;
}
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
costmap_2d::Costmap2D* costmap_; ///< @brief The costmap the controller will use
MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
tf2_ros::Buffer* tf_; ///< @brief Used for transforming point clouds
std::string global_frame_; ///< @brief The frame in which the controller will run
double max_sensor_range_; ///< @brief Keep track of the effective maximum range of our sensors
nav_msgs::Odometry base_odom_; ///< @brief Used to get the velocity of the robot
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
double rot_stopped_velocity_, trans_stopped_velocity_;
double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
bool prune_plan_;
boost::recursive_mutex odom_lock_;
double max_vel_th_, min_vel_th_;
double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
double sim_period_;
bool rotating_to_goal_;
bool reached_goal_;
bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
ros::Publisher g_plan_pub_, l_plan_pub_;
dynamic_reconfigure::Server<BaseLocalPlannerConfig> *dsrv_;
base_local_planner::BaseLocalPlannerConfig default_config_;
bool setup_;
bool initialized_;
base_local_planner::OdometryHelperRos odom_helper_;
std::vector<geometry_msgs::Point> footprint_spec_;
};
};
#endif

74
navigations/base_local_planner/include/base_local_planner/trajectory_sample_generator.h Executable file
View File

@@ -0,0 +1,74 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef TRAJECTORY_SAMPLE_GENERATOR_H_
#define TRAJECTORY_SAMPLE_GENERATOR_H_
#include <base_local_planner/trajectory.h>
namespace base_local_planner {
/**
* @class TrajectorySampleGenerator
* @brief Provides an interface for navigation trajectory generators
*/
class TrajectorySampleGenerator {
public:
/**
* Whether this generator can create more trajectories
*/
virtual bool hasMoreTrajectories() = 0;
/**
* Whether this generator can create more trajectories
*/
virtual bool nextTrajectory(Trajectory &traj) = 0;
/**
* @brief Virtual destructor for the interface
*/
virtual ~TrajectorySampleGenerator() {}
protected:
TrajectorySampleGenerator() {}
};
} // end namespace
#endif /* TRAJECTORY_SAMPLE_GENERATOR_H_ */

71
navigations/base_local_planner/include/base_local_planner/trajectory_search.h Executable file
View File

@@ -0,0 +1,71 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#ifndef TRAJECTORY_SEARCH_H_
#define TRAJECTORY_SEARCH_H_
#include <base_local_planner/trajectory.h>
namespace base_local_planner {
/**
* @class TrajectorySearch
* @brief Interface for modules finding a trajectory to use for navigation commands next
*/
class TrajectorySearch {
public:
/**
* searches the space of allowed trajectory and
* returns one considered the optimal given the
* constraints of the particular search.
*
* @param traj The container to write the result to
* @param all_explored pass NULL or a container to collect all trajectories for debugging (has a penalty)
*/
virtual bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) = 0;
virtual ~TrajectorySearch() {}
protected:
TrajectorySearch() {}
};
}
#endif /* TRAJECTORY_SEARCH_H_ */

64
navigations/base_local_planner/include/base_local_planner/twirling_cost_function.h Executable file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Open Source Robotics Foundation, 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: Morgan Quigley
*********************************************************************/
#ifndef TWIRLING_COST_FUNCTION_H
#define TWIRLING_COST_FUNCTION_H
#include <base_local_planner/trajectory_cost_function.h>
namespace base_local_planner {
/**
* This class provides a cost based on how much a robot "twirls" on its
* way to the goal. With differential-drive robots, there isn't a choice,
* but with holonomic or near-holonomic robots, sometimes a robot spins
* more than you'd like on its way to a goal. This class provides a way
* to assign a penalty purely to rotational velocities.
*/
class TwirlingCostFunction: public base_local_planner::TrajectoryCostFunction {
public:
TwirlingCostFunction() {}
~TwirlingCostFunction() {}
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
};
} /* namespace base_local_planner */
#endif /* TWIRLING_COST_FUNCTION_H_ */

99
navigations/base_local_planner/include/base_local_planner/velocity_iterator.h Executable file
View File

@@ -0,0 +1,99 @@
/*********************************************************************
*
* 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
*********************************************************************/
#ifndef DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#define DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#include <algorithm>
#include <cmath>
#include <vector>
namespace base_local_planner {
/**
* We use the class to get even sized samples between min and max, inluding zero if it is not included (and range goes from negative to positive
*/
class VelocityIterator {
public:
VelocityIterator(double min, double max, int num_samples):
current_index(0)
{
if (min == max) {
samples_.push_back(min);
} else {
num_samples = std::max(2, num_samples);
// e.g. for 4 samples, split distance in 3 even parts
double step_size = (max - min) / double(std::max(1, (num_samples - 1)));
// we make sure to avoid rounding errors around min and max.
double current;
double next = min;
for (int j = 0; j < num_samples - 1; ++j) {
current = next;
next += step_size;
samples_.push_back(current);
// if 0 is among samples, this is never true. Else it inserts a 0 between the positive and negative samples
if ((current < 0) && (next > 0)) {
samples_.push_back(0.0);
}
}
samples_.push_back(max);
}
}
double getVelocity(){
return samples_.at(current_index);
}
VelocityIterator& operator++(int){
current_index++;
return *this;
}
void reset(){
current_index = 0;
}
bool isFinished(){
return current_index >= samples_.size();
}
private:
std::vector<double> samples_;
unsigned int current_index;
};
};
#endif

179
navigations/base_local_planner/include/base_local_planner/voxel_grid_model.h Executable file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#define TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#include <vector>
#include <list>
#include <cfloat>
#include <geometry_msgs/Point.h>
#include <costmap_2d/observation.h>
#include <base_local_planner/world_model.h>
//voxel grid stuff
#include <voxel_grid/voxel_grid.h>
namespace base_local_planner {
/**
* @class VoxelGridModel
* @brief A class that implements the WorldModel interface to provide grid
* based collision checks for the trajectory controller using a 3D voxel grid.
*/
class VoxelGridModel : public WorldModel {
public:
/**
* @brief Constructor for the VoxelGridModel
* @param size_x The x size of the map
* @param size_y The y size of the map
* @param size_z The z size of the map... up to 32 cells
* @param xy_resolution The horizontal resolution of the map in meters/cell
* @param z_resolution The vertical resolution of the map in meters/cell
* @param origin_x The x value of the origin of the map
* @param origin_y The y value of the origin of the map
* @param origin_z The z value of the origin of the map
* @param max_z The maximum height for an obstacle to be added to the grid
* @param obstacle_range The maximum distance for obstacles to be added to the grid
*/
VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range);
/**
* @brief Destructor for the world model
*/
virtual ~VoxelGridModel(){}
/**
* @brief Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
using WorldModel::footprintCost;
/**
* @brief The costmap already keeps track of world observations, so for this world model this method does nothing
* @param footprint The footprint of the robot in its current location
* @param observations The observations from various sensors
* @param laser_scan The scans used to clear freespace
*/
void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
* @brief Function copying the Voxel points into a point cloud
* @param cloud the point cloud to copy data to. It has the usual x,y,z channels
*/
void getPoints(sensor_msgs::PointCloud2& cloud);
private:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
* @param x0 The x position of the first cell in grid coordinates
* @param y0 The y position of the first cell in grid coordinates
* @param x1 The x position of the second cell in grid coordinates
* @param y1 The y position of the second cell in grid coordinates
* @return A positive cost for a legal line... negative otherwise
*/
double lineCost(int x0, int x1, int y0, int y1);
/**
* @brief Checks the cost of a point in the costmap
* @param x The x position of the point in cell coordinates
* @param y The y position of the point in cell coordinates
* @return A positive cost for a legal point... negative otherwise
*/
double pointCost(int x, int y);
void removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range);
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_) / xy_resolution_);
my = (int) ((wy - origin_y_) / xy_resolution_);
mz = (int) ((wz - origin_z_) / z_resolution_);
return true;
}
inline bool worldToMap2D(double wx, double wy, unsigned int& mx, unsigned int& my){
if(wx < origin_x_ || wy < origin_y_)
return false;
mx = (int) ((wx - origin_x_) / xy_resolution_);
my = (int) ((wy - origin_y_) / xy_resolution_);
return true;
}
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) * xy_resolution_;
wy = origin_y_ + (my + 0.5) * xy_resolution_;
wz = origin_z_ + (mz + 0.5) * z_resolution_;
}
inline void mapToWorld2D(unsigned int mx, unsigned int my, double& wx, double& wy){
//returns the center point of the cell
wx = origin_x_ + (mx + 0.5) * xy_resolution_;
wy = origin_y_ + (my + 0.5) * xy_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));
}
inline void insert(const geometry_msgs::Point32& pt){
unsigned int cell_x, cell_y, cell_z;
if(!worldToMap3D(pt.x, pt.y, pt.z, cell_x, cell_y, cell_z))
return;
obstacle_grid_.markVoxel(cell_x, cell_y, cell_z);
}
voxel_grid::VoxelGrid obstacle_grid_;
double xy_resolution_;
double z_resolution_;
double origin_x_;
double origin_y_;
double origin_z_;
double max_z_; ///< @brief The height cutoff for adding points as obstacles
double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
};
};
#endif

114
navigations/base_local_planner/include/base_local_planner/world_model.h Executable file
View File

@@ -0,0 +1,114 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#define TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#include <vector>
#include <costmap_2d/observation.h>
#include <costmap_2d/footprint.h>
#include <geometry_msgs/Point.h>
#include <base_local_planner/planar_laser_scan.h>
namespace base_local_planner {
/**
* @class WorldModel
* @brief An interface the trajectory controller uses to interact with the
* world regardless of the underlying world model.
*/
class WorldModel{
public:
/**
* @brief Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise:
* -1 if footprint covers at least a lethal obstacle cell, or
* -2 if footprint covers at least a no-information cell, or
* -3 if footprint is partially or totally outside of the map
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius) = 0;
double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0){
double cos_th = cos(theta);
double sin_th = sin(theta);
std::vector<geometry_msgs::Point> oriented_footprint;
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);
}
geometry_msgs::Point robot_position;
robot_position.x = x;
robot_position.y = y;
if(inscribed_radius==0.0){
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius);
}
return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
}
/**
* @brief Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius, double extra) {
return footprintCost(position, footprint, inscribed_radius, circumscribed_radius);
}
/**
* @brief Subclass will implement a destructor
*/
virtual ~WorldModel(){}
protected:
WorldModel(){}
};
};
#endif

2
navigations/base_local_planner/msg/Position2DInt.msg Executable file
View File

@@ -0,0 +1,2 @@
int64 x
int64 y

52
navigations/base_local_planner/package.xml Executable file
View File

@@ -0,0 +1,52 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>base_local_planner</name>
<version>1.17.3</version>
<description>
This package provides implementations of the Trajectory Rollout and Dynamic Window approaches to local robot navigation on a plane. Given a plan to follow and a costmap, the controller produces velocity commands to send to a mobile base. This package supports both holonomic and non-holonomic robots, any robot footprint that can be represented as a convex polygon or circle, and exposes its configuration as ROS parameters that can be set in a launch file. This package's ROS wrapper adheres to the BaseLocalPlanner interface specified in the <a href="http://wiki.ros.org/nav_core">nav_core</a> package.
</description>
<author>Eitan Marder-Eppstein</author>
<author>Eric Perko</author>
<author>contradict@gmail.com</author>
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/base_local_planner</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<depend>angles</depend>
<depend>costmap_2d</depend>
<depend>dynamic_reconfigure</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>rosconsole</depend>
<depend>roscpp</depend>
<depend>rospy</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<depend>voxel_grid</depend>
<exec_depend>message_runtime</exec_depend>
<test_depend>rosunit</test_depend>
<export>
<nav_core plugin="${prefix}/blp_plugin.xml" />
</export>
</package>

10
navigations/base_local_planner/setup.py Executable file
View File

@@ -0,0 +1,10 @@
#!/usr/bin/env python
from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages = ['local_planner_limits'],
package_dir = {'': 'src'},
)
setup(**d)

145
navigations/base_local_planner/src/costmap_model.cpp Executable file
View File

@@ -0,0 +1,145 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 <base_local_planner/line_iterator.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/cost_values.h>
using namespace std;
using namespace costmap_2d;
namespace base_local_planner {
CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius){
// returns:
// -1 if footprint covers at least a lethal obstacle cell, or
// -2 if footprint covers at least a no-information cell, or
// -3 if footprint is [partially] outside of the map, or
// a positive value for traversable space
//used to put things into grid coordinates
unsigned int cell_x, cell_y;
//get the cell coord of the center point of the robot
if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
return -3.0;
//if number of points in the footprint is less than 3, we'll just assume a circular robot
if(footprint.size() < 3){
unsigned char cost = costmap_.getCost(cell_x, cell_y);
if(cost == NO_INFORMATION)
return -2.0;
if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
return -1.0;
return cost;
}
//now we really have to lay down the footprint in the costmap grid
unsigned int x0, x1, y0, y1;
double line_cost = 0.0;
double footprint_cost = 0.0;
//we need to rasterize each line in the footprint
for(unsigned int i = 0; i < footprint.size() - 1; ++i){
//get the cell coord of the first point
if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
return -3.0;
//get the cell coord of the second point
if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
return -3.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
//if there is an obstacle that hits the line... we know that we can return false right away
if(line_cost < 0)
return line_cost;
}
//we also need to connect the first point in the footprint to the last point
//get the cell coord of the last point
if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
return -3.0;
//get the cell coord of the first point
if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
return -3.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
if(line_cost < 0)
return line_cost;
//if all line costs are legal... then we can return that the footprint is legal
return footprint_cost;
}
//calculate the cost of a ray-traced line
double CostmapModel::lineCost(int x0, int x1, int y0, int y1) const {
double line_cost = 0.0;
double point_cost = -1.0;
for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() )
{
point_cost = pointCost( line.getX(), line.getY() ); //Score the current point
if(point_cost < 0)
return point_cost;
if(line_cost < point_cost)
line_cost = point_cost;
}
return line_cost;
}
double CostmapModel::pointCost(int x, int y) const {
unsigned char cost = costmap_.getCost(x, y);
//if the cell is in an obstacle the path is invalid
if(cost == NO_INFORMATION)
return -2;
if(cost == LETHAL_OBSTACLE)
return -1;
return cost;
}
};

248
navigations/base_local_planner/src/footprint_helper.cpp Executable file
View File

@@ -0,0 +1,248 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#include <base_local_planner/footprint_helper.h>
namespace base_local_planner {
FootprintHelper::FootprintHelper() {
// TODO Auto-generated constructor stub
}
FootprintHelper::~FootprintHelper() {
// TODO Auto-generated destructor stub
}
void FootprintHelper::getLineCells(int x0, int x1, int y0, int y1, std::vector<base_local_planner::Position2DInt>& pts) {
//Bresenham Ray-Tracing
int deltax = abs(x1 - x0); // The difference between the x's
int deltay = abs(y1 - y0); // The difference between the y's
int x = x0; // Start x off at the first pixel
int y = y0; // Start y off at the first pixel
int xinc1, xinc2, yinc1, yinc2;
int den, num, numadd, numpixels;
base_local_planner::Position2DInt pt;
if (x1 >= x0) // The x-values are increasing
{
xinc1 = 1;
xinc2 = 1;
}
else // The x-values are decreasing
{
xinc1 = -1;
xinc2 = -1;
}
if (y1 >= y0) // The y-values are increasing
{
yinc1 = 1;
yinc2 = 1;
}
else // The y-values are decreasing
{
yinc1 = -1;
yinc2 = -1;
}
if (deltax >= deltay) // There is at least one x-value for every y-value
{
xinc1 = 0; // Don't change the x when numerator >= denominator
yinc2 = 0; // Don't change the y for every iteration
den = deltax;
num = deltax / 2;
numadd = deltay;
numpixels = deltax; // There are more x-values than y-values
}
else // There is at least one y-value for every x-value
{
xinc2 = 0; // Don't change the x for every iteration
yinc1 = 0; // Don't change the y when numerator >= denominator
den = deltay;
num = deltay / 2;
numadd = deltax;
numpixels = deltay; // There are more y-values than x-values
}
for (int curpixel = 0; curpixel <= numpixels; curpixel++)
{
pt.x = x; //Draw the current pixel
pt.y = y;
pts.push_back(pt);
num += numadd; // Increase the numerator by the top of the fraction
if (num >= den) // Check if numerator >= denominator
{
num -= den; // Calculate the new numerator value
x += xinc1; // Change the x as appropriate
y += yinc1; // Change the y as appropriate
}
x += xinc2; // Change the x as appropriate
y += yinc2; // Change the y as appropriate
}
}
void FootprintHelper::getFillCells(std::vector<base_local_planner::Position2DInt>& footprint){
//quick bubble sort to sort pts by x
base_local_planner::Position2DInt swap, pt;
unsigned int i = 0;
while (i < footprint.size() - 1) {
if (footprint[i].x > footprint[i + 1].x) {
swap = footprint[i];
footprint[i] = footprint[i + 1];
footprint[i + 1] = swap;
if(i > 0) {
--i;
}
} else {
++i;
}
}
i = 0;
base_local_planner::Position2DInt min_pt;
base_local_planner::Position2DInt max_pt;
unsigned int min_x = footprint[0].x;
unsigned int max_x = footprint[footprint.size() -1].x;
//walk through each column and mark cells inside the footprint
for (unsigned int x = min_x; x <= max_x; ++x) {
if (i >= footprint.size() - 1) {
break;
}
if (footprint[i].y < footprint[i + 1].y) {
min_pt = footprint[i];
max_pt = footprint[i + 1];
} else {
min_pt = footprint[i + 1];
max_pt = footprint[i];
}
i += 2;
while (i < footprint.size() && footprint[i].x == x) {
if(footprint[i].y < min_pt.y) {
min_pt = footprint[i];
} else if(footprint[i].y > max_pt.y) {
max_pt = footprint[i];
}
++i;
}
//loop though cells in the column
for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
pt.x = x;
pt.y = y;
footprint.push_back(pt);
}
}
}
/**
* get the cellsof a footprint at a given position
*/
std::vector<base_local_planner::Position2DInt> FootprintHelper::getFootprintCells(
Eigen::Vector3f pos,
std::vector<geometry_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D& costmap,
bool fill){
double x_i = pos[0];
double y_i = pos[1];
double theta_i = pos[2];
std::vector<base_local_planner::Position2DInt> footprint_cells;
//if we have no footprint... do nothing
if (footprint_spec.size() <= 1) {
unsigned int mx, my;
if (costmap.worldToMap(x_i, y_i, mx, my)) {
Position2DInt center;
center.x = mx;
center.y = my;
footprint_cells.push_back(center);
}
return footprint_cells;
}
//pre-compute cos and sin values
double cos_th = cos(theta_i);
double sin_th = sin(theta_i);
double new_x, new_y;
unsigned int x0, y0, x1, y1;
unsigned int last_index = footprint_spec.size() - 1;
for (unsigned int i = 0; i < last_index; ++i) {
//find the cell coordinates of the first segment point
new_x = x_i + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_y = y_i + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
//find the cell coordinates of the second segment point
new_x = x_i + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
new_y = y_i + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
}
//we need to close the loop, so we also have to raytrace from the last pt to first pt
new_x = x_i + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
new_y = y_i + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
new_x = x_i + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
new_y = y_i + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
if(fill) {
getFillCells(footprint_cells);
}
return footprint_cells;
}
} /* namespace base_local_planner */

245
navigations/base_local_planner/src/goal_functions.cpp Executable file
View File

@@ -0,0 +1,245 @@
/*********************************************************************
*
* 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 <base_local_planner/goal_functions.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#ifdef _MSC_VER
#define GOAL_ATTRIBUTE_UNUSED
#else
#define GOAL_ATTRIBUTE_UNUSED __attribute__ ((unused))
#endif
namespace base_local_planner {
double getGoalPositionDistance(const geometry_msgs::PoseStamped& global_pose, double goal_x, double goal_y) {
return hypot(goal_x - global_pose.pose.position.x, goal_y - global_pose.pose.position.y);
}
double getGoalOrientationAngleDifference(const geometry_msgs::PoseStamped& global_pose, double goal_th) {
double yaw = tf2::getYaw(global_pose.pose.orientation);
return angles::shortest_angular_distance(yaw, goal_th);
}
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub) {
//given an empty path we won't do anything
if(path.empty())
return;
//create a path message
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = path[0].header.frame_id;
gui_path.header.stamp = path[0].header.stamp;
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
for(unsigned int i=0; i < path.size(); i++){
gui_path.poses[i] = path[i];
}
pub.publish(gui_path);
}
void prunePlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
while(it != plan.end()){
const geometry_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
double x_diff = global_pose.pose.position.x - w.pose.position.x;
double y_diff = global_pose.pose.position.y - w.pose.position.y;
double distance_sq = x_diff * x_diff + y_diff * y_diff;
if(distance_sq < 1){
ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.pose.position.x, global_pose.pose.position.y, w.pose.position.x, w.pose.position.y);
break;
}
it = plan.erase(it);
global_it = global_plan.erase(global_it);
}
}
bool transformGlobalPlan(
const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
transformed_plan.clear();
if (global_plan.empty()) {
ROS_ERROR("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
try {
// get plan_to_global_transform from plan frame to global_frame
geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5));
//let's get the pose of the robot in the frame of the plan
geometry_msgs::PoseStamped robot_pose;
tf.transform(global_pose, robot_pose, plan_pose.header.frame_id);
//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {
double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];
tf2::doTransform(pose, newer_pose, plan_to_global_transform);
transformed_plan.push_back(newer_pose);
double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
}
catch(tf2::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf2::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf2::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (!global_plan.empty())
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool getGoalPose(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, geometry_msgs::PoseStamped &goal_pose) {
if (global_plan.empty())
{
ROS_ERROR("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try{
geometry_msgs::TransformStamped transform = tf.lookupTransform(global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration(0.5));
tf2::doTransform(plan_goal_pose, goal_pose, transform);
}
catch(tf2::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf2::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf2::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool isGoalReached(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap GOAL_ATTRIBUTE_UNUSED,
const std::string& global_frame,
geometry_msgs::PoseStamped& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance){
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
getGoalPose(tf, global_plan, global_frame, goal_pose);
double goal_x = goal_pose.pose.position.x;
double goal_y = goal_pose.pose.position.y;
double goal_th = tf2::getYaw(goal_pose.pose.orientation);
//check to see if we've reached the goal position
if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
//check to see if the goal orientation has been reached
if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) {
//make sure that we're actually stopped before returning success
if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
return true;
}
}
return false;
}
bool stopped(const nav_msgs::Odometry& base_odom,
const double& rot_stopped_velocity, const double& trans_stopped_velocity){
return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
&& fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
&& fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
}
};

278
navigations/base_local_planner/src/latched_stop_rotate_controller.cpp Executable file
View File

@@ -0,0 +1,278 @@
/*
* latched_stop_rotate_controller.cpp
*
* Created on: Apr 16, 2012
* Author: tkruse
*/
#include <base_local_planner/latched_stop_rotate_controller.h>
#include <cmath>
#include <Eigen/Core>
#include <angles/angles.h>
#include <nav_msgs/Odometry.h>
#include <base_local_planner/goal_functions.h>
#include <base_local_planner/local_planner_limits.h>
#include <tf2/utils.h>
namespace base_local_planner {
LatchedStopRotateController::LatchedStopRotateController(const std::string& name) {
ros::NodeHandle private_nh("~/" + name);
private_nh.param("latch_xy_goal_tolerance", latch_xy_goal_tolerance_, false);
rotating_to_goal_ = false;
}
LatchedStopRotateController::~LatchedStopRotateController() {}
/**
* returns true if we have passed the goal position.
* Meaning we might have overshot on the position beyond tolerance, yet still return true.
* Also goal orientation might not yet be true
*/
bool LatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util,
const geometry_msgs::PoseStamped& global_pose) {
double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
if ( ! planner_util->getGoal(goal_pose)) {
return false;
}
double goal_x = goal_pose.pose.position.x;
double goal_y = goal_pose.pose.position.y;
//check to see if we've reached the goal position
if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
xy_tolerance_latch_ = true;
return true;
}
return false;
}
/**
* returns true if we have passed the goal position and have reached goal orientation.
* Meaning we might have overshot on the position beyond tolerance, yet still return true.
*/
bool LatchedStopRotateController::isGoalReached(LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper,
const geometry_msgs::PoseStamped& global_pose) {
double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
double theta_stopped_vel = planner_util->getCurrentLimits().theta_stopped_vel;
double trans_stopped_vel = planner_util->getCurrentLimits().trans_stopped_vel;
//copy over the odometry information
nav_msgs::Odometry base_odom;
odom_helper.getOdom(base_odom);
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
if ( ! planner_util->getGoal(goal_pose)) {
return false;
}
double goal_x = goal_pose.pose.position.x;
double goal_y = goal_pose.pose.position.y;
base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
//check to see if we've reached the goal position
if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
//if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
//just rotate in place
if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_) {
ROS_DEBUG("Goal position reached (check), stopping and turning in place");
xy_tolerance_latch_ = true;
}
double goal_th = tf2::getYaw(goal_pose.pose.orientation);
double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
//check to see if the goal orientation has been reached
if (fabs(angle) <= limits.yaw_goal_tolerance) {
//make sure that we're actually stopped before returning success
if (base_local_planner::stopped(base_odom, theta_stopped_vel, trans_stopped_vel)) {
return true;
}
}
}
return false;
}
bool LatchedStopRotateController::stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check) {
//slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
//but we'll use a tenth of a second to be consistent with the implementation of the local planner.
double vx = sign(robot_vel.pose.position.x) * std::max(0.0, (fabs(robot_vel.pose.position.x) - acc_lim[0] * sim_period));
double vy = sign(robot_vel.pose.position.y) * std::max(0.0, (fabs(robot_vel.pose.position.y) - acc_lim[1] * sim_period));
double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim[2] * sim_period));
//we do want to check whether or not the command is valid
double yaw = tf2::getYaw(global_pose.pose.orientation);
bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
Eigen::Vector3f(vx, vy, vth));
//if we have a valid command, we'll pass it on, otherwise we'll command all zeros
if(valid_cmd){
ROS_DEBUG_NAMED("latched_stop_rotate", "Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
cmd_vel.linear.x = vx;
cmd_vel.linear.y = vy;
cmd_vel.angular.z = vth;
return true;
}
ROS_WARN("Stopping cmd in collision");
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
return false;
}
bool LatchedStopRotateController::rotateToGoal(
const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& robot_vel,
double goal_th,
geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
base_local_planner::LocalPlannerLimits& limits,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check) {
double yaw = tf2::getYaw(global_pose.pose.orientation);
double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
double v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, fabs(ang_diff)));
//take the acceleration limits of the robot into account
double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period;
double min_acc_vel = fabs(vel_yaw) - acc_lim[2] * sim_period;
v_theta_samp = std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
//we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits
double max_speed_to_stop = sqrt(2 * acc_lim[2] * fabs(ang_diff));
v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp));
v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, v_theta_samp));
if (ang_diff < 0) {
v_theta_samp = - v_theta_samp;
}
//we still want to lay down the footprint of the robot and check if the action is legal
bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
Eigen::Vector3f( 0.0, 0.0, v_theta_samp));
if (valid_cmd) {
ROS_DEBUG_NAMED("dwa_local_planner", "Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
cmd_vel.angular.z = v_theta_samp;
return true;
}
ROS_WARN("Rotation cmd in collision");
cmd_vel.angular.z = 0.0;
return false;
}
bool LatchedStopRotateController::computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
Eigen::Vector3f acc_lim,
double sim_period,
LocalPlannerUtil* planner_util,
OdometryHelperRos& odom_helper_,
const geometry_msgs::PoseStamped& global_pose,
boost::function<bool (Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f vel_samples)> obstacle_check) {
//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
if ( ! planner_util->getGoal(goal_pose)) {
ROS_ERROR("Could not get goal pose");
return false;
}
base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
//if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
//just rotate in place
if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_ ) {
ROS_INFO("Goal position reached, stopping and turning in place");
xy_tolerance_latch_ = true;
}
//check to see if the goal orientation has been reached
double goal_th = tf2::getYaw(goal_pose.pose.orientation);
double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
if (fabs(angle) <= limits.yaw_goal_tolerance) {
//set the velocity command to zero
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
rotating_to_goal_ = false;
} else {
ROS_DEBUG("Angle: %f Tolerance: %f", angle, limits.yaw_goal_tolerance);
geometry_msgs::PoseStamped robot_vel;
odom_helper_.getRobotVel(robot_vel);
nav_msgs::Odometry base_odom;
odom_helper_.getOdom(base_odom);
//if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, limits.theta_stopped_vel, limits.trans_stopped_vel)) {
if ( ! stopWithAccLimits(
global_pose,
robot_vel,
cmd_vel,
acc_lim,
sim_period,
obstacle_check)) {
ROS_INFO("Error when stopping.");
return false;
}
ROS_DEBUG("Stopping...");
}
//if we're stopped... then we want to rotate to goal
else {
//set this so that we know its OK to be moving
rotating_to_goal_ = true;
if ( ! rotateToGoal(
global_pose,
robot_vel,
goal_th,
cmd_vel,
acc_lim,
sim_period,
limits,
obstacle_check)) {
ROS_INFO("Error when rotating.");
return false;
}
ROS_DEBUG("Rotating...");
}
}
return true;
}
} /* namespace base_local_planner */

1
navigations/base_local_planner/src/local_planner_limits/.gitignore vendored Executable file
View File

@@ -0,0 +1 @@
*.pyc

41
navigations/base_local_planner/src/local_planner_limits/__init__.py Executable file
View File

@@ -0,0 +1,41 @@
# Generic set of parameters to use with base local planners
# To use:
#
# from local_planner_limits import add_generic_localplanner_params
# gen = ParameterGenerator()
# add_generic_localplanner_params(gen)
# ...
#
# Using these standard parameters instead of your own allows easier switching of local planners
# need this only for dataype declarations
from dynamic_reconfigure.parameter_generator_catkin import double_t, bool_t
def add_generic_localplanner_params(gen):
# velocities
gen.add("max_vel_trans", double_t, 0, "The absolute value of the maximum translational velocity for the robot in m/s", 0.55, 0)
gen.add("min_vel_trans", double_t, 0, "The absolute value of the minimum translational velocity for the robot in m/s", 0.1, 0)
gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55)
gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0)
gen.add("max_vel_y", double_t, 0, "The maximum y velocity for the robot in m/s", 0.1)
gen.add("min_vel_y", double_t, 0, "The minimum y velocity for the robot in m/s", -0.1)
gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0)
gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", 0.4, 0)
# acceleration
gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 2.5, 0, 20.0)
gen.add("acc_lim_y", double_t, 0, "The acceleration limit of the robot in the y direction", 2.5, 0, 20.0)
gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in the theta direction", 3.2, 0, 20.0)
gen.add("acc_lim_trans", double_t, 0, "The absolute value of the maximum translational acceleration for the robot in m/s^2", 0.1, 0)
gen.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False)
gen.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1)
gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1)
gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1)
gen.add("theta_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1)

128
navigations/base_local_planner/src/local_planner_util.cpp Executable file
View File

@@ -0,0 +1,128 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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 <base_local_planner/local_planner_util.h>
#include <base_local_planner/goal_functions.h>
namespace base_local_planner {
void LocalPlannerUtil::initialize(
tf2_ros::Buffer* tf,
costmap_2d::Costmap2D* costmap,
std::string global_frame) {
if(!initialized_) {
tf_ = tf;
costmap_ = costmap;
global_frame_ = global_frame;
initialized_ = true;
}
else{
ROS_WARN("Planner utils have already been initialized, doing nothing.");
}
}
void LocalPlannerUtil::reconfigureCB(LocalPlannerLimits &config, bool restore_defaults)
{
if(setup_ && restore_defaults) {
config = default_limits_;
}
if(!setup_) {
default_limits_ = config;
setup_ = true;
}
boost::mutex::scoped_lock l(limits_configuration_mutex_);
limits_ = LocalPlannerLimits(config);
}
costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() {
return costmap_;
}
LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() {
boost::mutex::scoped_lock l(limits_configuration_mutex_);
return limits_;
}
bool LocalPlannerUtil::getGoal(geometry_msgs::PoseStamped& goal_pose) {
//we assume the global goal is the last point in the global plan
return base_local_planner::getGoalPose(*tf_,
global_plan_,
global_frame_,
goal_pose);
}
bool LocalPlannerUtil::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if(!initialized_){
ROS_ERROR("Planner utils have not been initialized, please call initialize() first");
return false;
}
//reset the global plan
global_plan_.clear();
global_plan_ = orig_global_plan;
return true;
}
bool LocalPlannerUtil::getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan) {
//get the global plan in our frame
if(!base_local_planner::transformGlobalPlan(
*tf_,
global_plan_,
global_pose,
*costmap_,
global_frame_,
transformed_plan)) {
ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
}
//now we'll prune the plan based on the position of the robot
if(limits_.prune_plan) {
base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
}
return true;
}
} // namespace

52
navigations/base_local_planner/src/map_cell.cpp Executable file
View File

@@ -0,0 +1,52 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 <base_local_planner/map_cell.h>
namespace base_local_planner{
MapCell::MapCell()
: cx(0), cy(0),
target_dist(DBL_MAX),
target_mark(false),
within_robot(false)
{}
MapCell::MapCell(const MapCell& mc)
: cx(mc.cx), cy(mc.cy),
target_dist(mc.target_dist),
target_mark(mc.target_mark),
within_robot(mc.within_robot)
{}
};

309
navigations/base_local_planner/src/map_grid.cpp Executable file
View File

@@ -0,0 +1,309 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 <base_local_planner/map_grid.h>
#include <costmap_2d/cost_values.h>
using namespace std;
namespace base_local_planner{
MapGrid::MapGrid()
: size_x_(0), size_y_(0)
{
}
MapGrid::MapGrid(unsigned int size_x, unsigned int size_y)
: size_x_(size_x), size_y_(size_y)
{
commonInit();
}
MapGrid::MapGrid(const MapGrid& mg){
size_y_ = mg.size_y_;
size_x_ = mg.size_x_;
map_ = mg.map_;
}
void MapGrid::commonInit(){
//don't allow construction of zero size grid
ROS_ASSERT(size_y_ != 0 && size_x_ != 0);
map_.resize(size_y_ * size_x_);
//make each cell aware of its location in the grid
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
unsigned int id = size_x_ * i + j;
map_[id].cx = j;
map_[id].cy = i;
}
}
}
size_t MapGrid::getIndex(int x, int y){
return size_x_ * y + x;
}
MapGrid& MapGrid::operator= (const MapGrid& mg){
size_y_ = mg.size_y_;
size_x_ = mg.size_x_;
map_ = mg.map_;
return *this;
}
void MapGrid::sizeCheck(unsigned int size_x, unsigned int size_y){
if(map_.size() != size_x * size_y)
map_.resize(size_x * size_y);
if(size_x_ != size_x || size_y_ != size_y){
size_x_ = size_x;
size_y_ = size_y;
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
int index = size_x_ * i + j;
map_[index].cx = j;
map_[index].cy = i;
}
}
}
}
inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap){
//if the cell is an obstacle set the max path distance
unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION)){
check_cell->target_dist = obstacleCosts();
return false;
}
double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist) {
check_cell->target_dist = new_target_dist;
}
return true;
}
//reset the path_dist and goal_dist fields for all cells
void MapGrid::resetPathDist(){
for(unsigned int i = 0; i < map_.size(); ++i) {
map_[i].target_dist = unreachableCellCosts();
map_[i].target_mark = false;
map_[i].within_robot = false;
}
}
void MapGrid::adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution) {
if (global_plan_in.size() == 0) {
return;
}
double last_x = global_plan_in[0].pose.position.x;
double last_y = global_plan_in[0].pose.position.y;
global_plan_out.push_back(global_plan_in[0]);
double min_sq_resolution = resolution * resolution;
for (unsigned int i = 1; i < global_plan_in.size(); ++i) {
double loop_x = global_plan_in[i].pose.position.x;
double loop_y = global_plan_in[i].pose.position.y;
double sqdist = (loop_x - last_x) * (loop_x - last_x) + (loop_y - last_y) * (loop_y - last_y);
if (sqdist > min_sq_resolution) {
int steps = ceil((sqrt(sqdist)) / resolution);
// add a points in-between
double deltax = (loop_x - last_x) / steps;
double deltay = (loop_y - last_y) / steps;
// TODO: Interpolate orientation
for (int j = 1; j < steps; ++j) {
geometry_msgs::PoseStamped pose;
pose.pose.position.x = last_x + j * deltax;
pose.pose.position.y = last_y + j * deltay;
pose.pose.position.z = global_plan_in[i].pose.position.z;
pose.pose.orientation = global_plan_in[i].pose.orientation;
pose.header = global_plan_in[i].header;
global_plan_out.push_back(pose);
}
}
global_plan_out.push_back(global_plan_in[i]);
last_x = loop_x;
last_y = loop_y;
}
}
//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size()) {
ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
MapCell& current = getCell(map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(&current);
started_path = true;
} else if (started_path) {
break;
}
}
if (!started_path) {
ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
computeTargetDistance(path_dist_queue, costmap);
}
//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan) {
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
} else {
if (started_path) {
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path) {
ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
queue<MapCell*> path_dist_queue;
if (local_goal_x >= 0 && local_goal_y >= 0) {
MapCell& current = getCell(local_goal_x, local_goal_y);
costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(&current);
}
computeTargetDistance(path_dist_queue, costmap);
}
void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
MapCell* current_cell;
MapCell* check_cell;
unsigned int last_col = size_x_ - 1;
unsigned int last_row = size_y_ - 1;
while(!dist_queue.empty()){
current_cell = dist_queue.front();
dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell - 1;
if(!check_cell->target_mark){
//mark the cell as visisted
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cx < last_col){
check_cell = current_cell + 1;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cy > 0){
check_cell = current_cell - size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cy < last_row){
check_cell = current_cell + size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
}
}
};

131
navigations/base_local_planner/src/map_grid_cost_function.cpp Executable file
View File

@@ -0,0 +1,131 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#include <base_local_planner/map_grid_cost_function.h>
namespace base_local_planner {
MapGridCostFunction::MapGridCostFunction(costmap_2d::Costmap2D* costmap,
double xshift,
double yshift,
bool is_local_goal_function,
CostAggregationType aggregationType) :
costmap_(costmap),
map_(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()),
aggregationType_(aggregationType),
xshift_(xshift),
yshift_(yshift),
is_local_goal_function_(is_local_goal_function),
stop_on_failure_(true) {}
void MapGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
target_poses_ = target_poses;
}
bool MapGridCostFunction::prepare() {
map_.resetPathDist();
if (is_local_goal_function_) {
map_.setLocalGoal(*costmap_, target_poses_);
} else {
map_.setTargetCells(*costmap_, target_poses_);
}
return true;
}
double MapGridCostFunction::getCellCosts(unsigned int px, unsigned int py) {
double grid_dist = map_(px, py).target_dist;
return grid_dist;
}
double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0.0;
if (aggregationType_ == Product) {
cost = 1.0;
}
double px, py, pth;
unsigned int cell_x, cell_y;
double grid_dist;
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
// translate point forward if specified
if (xshift_ != 0.0) {
px = px + xshift_ * cos(pth);
py = py + xshift_ * sin(pth);
}
// translate point sideways if specified
if (yshift_ != 0.0) {
px = px + yshift_ * cos(pth + M_PI_2);
py = py + yshift_ * sin(pth + M_PI_2);
}
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
//we're off the map
ROS_WARN("Off Map %f, %f", px, py);
return -4.0;
}
grid_dist = getCellCosts(cell_x, cell_y);
//if a point on this trajectory has no clear path to the goal... it may be invalid
if (stop_on_failure_) {
if (grid_dist == map_.obstacleCosts()) {
return -3.0;
} else if (grid_dist == map_.unreachableCellCosts()) {
return -2.0;
}
}
switch( aggregationType_ ) {
case Last:
cost = grid_dist;
break;
case Sum:
cost += grid_dist;
break;
case Product:
if (cost > 0) {
cost *= grid_dist;
}
break;
}
}
return cost;
}
} /* namespace base_local_planner */

95
navigations/base_local_planner/src/map_grid_visualizer.cpp Executable file
View File

@@ -0,0 +1,95 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Eric Perko
* 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 Eric Perko 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 <base_local_planner/map_grid_visualizer.h>
#include <base_local_planner/map_cell.h>
#include <vector>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
namespace base_local_planner {
MapGridVisualizer::MapGridVisualizer() {}
void MapGridVisualizer::initialize(const std::string& name, std::string frame_id, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function) {
name_ = name;
frame_id_ = frame_id;
cost_function_ = cost_function;
ns_nh_ = ros::NodeHandle("~/" + name_);
pub_ = ns_nh_.advertise<sensor_msgs::PointCloud2>("cost_cloud", 1);
}
void MapGridVisualizer::publishCostCloud(const costmap_2d::Costmap2D* costmap_p_) {
sensor_msgs::PointCloud2 cost_cloud;
cost_cloud.header.frame_id = frame_id_;
cost_cloud.header.stamp = ros::Time::now();
sensor_msgs::PointCloud2Modifier cloud_mod(cost_cloud);
cloud_mod.setPointCloud2Fields(7, "x", 1, sensor_msgs::PointField::FLOAT32,
"y", 1, sensor_msgs::PointField::FLOAT32,
"z", 1, sensor_msgs::PointField::FLOAT32,
"path_cost", 1, sensor_msgs::PointField::FLOAT32,
"goal_cost", 1, sensor_msgs::PointField::FLOAT32,
"occ_cost", 1, sensor_msgs::PointField::FLOAT32,
"total_cost", 1, sensor_msgs::PointField::FLOAT32);
unsigned int x_size = costmap_p_->getSizeInCellsX();
unsigned int y_size = costmap_p_->getSizeInCellsY();
double z_coord = 0.0;
double x_coord, y_coord;
cloud_mod.resize(x_size * y_size);
sensor_msgs::PointCloud2Iterator<float> iter_x(cost_cloud, "x");
float path_cost, goal_cost, occ_cost, total_cost;
for (unsigned int cx = 0; cx < x_size; cx++) {
for (unsigned int cy = 0; cy < y_size; cy++) {
costmap_p_->mapToWorld(cx, cy, x_coord, y_coord);
if (cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
iter_x[0] = x_coord;
iter_x[1] = y_coord;
iter_x[2] = z_coord;
iter_x[3] = path_cost;
iter_x[4] = goal_cost;
iter_x[5] = occ_cost;
iter_x[6] = total_cost;
++iter_x;
}
}
}
pub_.publish(cost_cloud);
ROS_DEBUG("Cost PointCloud published");
}
};

152
navigations/base_local_planner/src/obstacle_cost_function.cpp Executable file
View File

@@ -0,0 +1,152 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#include <base_local_planner/obstacle_cost_function.h>
#include <cmath>
#include <Eigen/Core>
#include <ros/console.h>
namespace base_local_planner {
ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap)
: costmap_(costmap), sum_scores_(false) {
if (costmap != NULL) {
world_model_ = new base_local_planner::CostmapModel(*costmap_);
}
}
ObstacleCostFunction::~ObstacleCostFunction() {
if (world_model_ != NULL) {
delete world_model_;
}
}
void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
// TODO: move this to prepare if possible
max_trans_vel_ = max_trans_vel;
max_scaling_factor_ = max_scaling_factor;
scaling_speed_ = scaling_speed;
}
void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
footprint_spec_ = footprint_spec;
}
bool ObstacleCostFunction::prepare() {
return true;
}
double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
double cost = 0;
double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
double px, py, pth;
if (footprint_spec_.size() == 0) {
// Bug, should never happen
ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
return -9;
}
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
double f_cost = footprintCost(px, py, pth,
scale, footprint_spec_,
costmap_, world_model_);
if(f_cost < 0){
return f_cost;
}
if(sum_scores_)
cost += f_cost;
else
cost = std::max(cost, f_cost);
}
return cost;
}
double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
double vmag = hypot(traj.xv_, traj.yv_);
//if we're over a certain speed threshold, we'll scale the robot's
//footprint to make it either slow down or stay further from walls
double scale = 1.0;
if (vmag > scaling_speed) {
//scale up to the max scaling factor linearly... this could be changed later
double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
scale = max_scaling_factor * ratio + 1.0;
}
return scale;
}
double ObstacleCostFunction::footprintCost (
const double& x,
const double& y,
const double& th,
double scale,
std::vector<geometry_msgs::Point> footprint_spec,
costmap_2d::Costmap2D* costmap,
base_local_planner::WorldModel* world_model) {
std::vector<geometry_msgs::Point> scaled_footprint;
for(unsigned int i = 0; i < footprint_spec.size(); ++i) {
geometry_msgs::Point new_pt;
new_pt.x = scale * footprint_spec[i].x;
new_pt.y = scale * footprint_spec[i].y;
scaled_footprint.push_back(new_pt);
}
//check if the footprint is legal
// TODO: Cache inscribed radius
double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint);
if (footprint_cost < 0) {
return -6.0;
}
unsigned int cell_x, cell_y;
//we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
return -7.0;
}
double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
return occ_cost;
}
} /* namespace base_local_planner */

106
navigations/base_local_planner/src/odometry_helper_ros.cpp Executable file
View File

@@ -0,0 +1,106 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of 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: TKruse
*********************************************************************/
#include <base_local_planner/odometry_helper_ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/convert.h>
namespace base_local_planner {
OdometryHelperRos::OdometryHelperRos(std::string odom_topic) {
setOdomTopic( odom_topic );
}
void OdometryHelperRos::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
ROS_INFO_ONCE("odom received!");
//we assume that the odometry is published in the frame of the base
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
base_odom_.child_frame_id = msg->child_frame_id;
// ROS_DEBUG_NAMED("dwa_local_planner", "In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
// base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
}
//copy over the odometry information
void OdometryHelperRos::getOdom(nav_msgs::Odometry& base_odom) {
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom = base_odom_;
}
void OdometryHelperRos::getRobotVel(geometry_msgs::PoseStamped& robot_vel) {
// Set current velocities from odometry
geometry_msgs::Twist global_vel;
{
boost::mutex::scoped_lock lock(odom_mutex_);
global_vel.linear.x = base_odom_.twist.twist.linear.x;
global_vel.linear.y = base_odom_.twist.twist.linear.y;
global_vel.angular.z = base_odom_.twist.twist.angular.z;
robot_vel.header.frame_id = base_odom_.child_frame_id;
}
robot_vel.pose.position.x = global_vel.linear.x;
robot_vel.pose.position.y = global_vel.linear.y;
robot_vel.pose.position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, global_vel.angular.z);
tf2::convert(q, robot_vel.pose.orientation);
robot_vel.header.stamp = ros::Time();
}
void OdometryHelperRos::setOdomTopic(std::string odom_topic)
{
if( odom_topic != odom_topic_ )
{
odom_topic_ = odom_topic;
if( odom_topic_ != "" )
{
ros::NodeHandle gn;
odom_sub_ = gn.subscribe<nav_msgs::Odometry>( odom_topic_, 1, [this](auto& msg){ odomCallback(msg); });
}
else
{
odom_sub_.shutdown();
}
}
}
} /* namespace base_local_planner */

Some files were not shown because too many files have changed in this diff Show More