git commit -m "first commit for v2"
This commit is contained in:
258
Localizations/Libraries/Ros/amcl/CHANGELOG.rst
Normal file
258
Localizations/Libraries/Ros/amcl/CHANGELOG.rst
Normal 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.
|
||||
|
||||
184
Localizations/Libraries/Ros/amcl/CMakeLists.txt
Normal file
184
Localizations/Libraries/Ros/amcl/CMakeLists.txt
Normal file
@@ -0,0 +1,184 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
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 amcl_lib
|
||||
)
|
||||
|
||||
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_library(amcl_lib
|
||||
src/amcl.cpp
|
||||
)
|
||||
add_dependencies(amcl_lib
|
||||
amcl_sensors
|
||||
amcl_map
|
||||
amcl_pf
|
||||
${${PROJECT_NAME}_EXPORTED_TARGETS}
|
||||
${catkin_EXPORTED_TARGETS}
|
||||
)
|
||||
target_link_libraries(amcl_lib
|
||||
amcl_sensors
|
||||
amcl_map
|
||||
amcl_pf
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(amcl src/amcl_node.cpp)
|
||||
add_dependencies(amcl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(amcl
|
||||
amcl_lib
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
amcl
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
amcl_sensors amcl_map amcl_pf amcl_lib
|
||||
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
Localizations/Libraries/Ros/amcl/cfg/AMCL.cfg
Normal file
77
Localizations/Libraries/Ros/amcl/cfg/AMCL.cfg
Normal 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
Localizations/Libraries/Ros/amcl/examples/amcl_diff.launch
Normal file
34
Localizations/Libraries/Ros/amcl/examples/amcl_diff.launch
Normal 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
Localizations/Libraries/Ros/amcl/examples/amcl_omni.launch
Normal file
34
Localizations/Libraries/Ros/amcl/examples/amcl_omni.launch
Normal 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>
|
||||
293
Localizations/Libraries/Ros/amcl/include/amcl/amcl.h
Normal file
293
Localizations/Libraries/Ros/amcl/include/amcl/amcl.h
Normal file
@@ -0,0 +1,293 @@
|
||||
#ifndef _AMCL_LIBRARY_H_INCLUDED_
|
||||
#define _AMCL_LIBRARY_H_INCLUDED_
|
||||
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
|
||||
|
||||
#include "amcl/map/map.h"
|
||||
#include "amcl/pf/pf.h"
|
||||
#include "amcl/sensors/amcl_odom.h"
|
||||
#include "amcl/sensors/amcl_laser.h"
|
||||
#include "amcl/portable_utils.hpp"
|
||||
|
||||
#include "ros/assert.h"
|
||||
|
||||
// roscpp
|
||||
#include "ros/ros.h"
|
||||
|
||||
// Messages that I need
|
||||
#include "sensor_msgs/LaserScan.h"
|
||||
#include "geometry_msgs/PoseWithCovarianceStamped.h"
|
||||
#include "geometry_msgs/PoseArray.h"
|
||||
#include "geometry_msgs/Pose.h"
|
||||
#include "geometry_msgs/PoseStamped.h"
|
||||
#include "geometry_msgs/PoseWithCovarianceStamped.h"
|
||||
#include "nav_msgs/GetMap.h"
|
||||
#include "nav_msgs/SetMap.h"
|
||||
#include "std_srvs/Empty.h"
|
||||
|
||||
// For transform support
|
||||
#include "tf2/LinearMath/Transform.h"
|
||||
#include "tf2/convert.h"
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/message_filter.h"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "message_filters/subscriber.h"
|
||||
|
||||
// Dynamic_reconfigure
|
||||
#include "dynamic_reconfigure/server.h"
|
||||
#include "amcl/AMCLConfig.h"
|
||||
|
||||
// Allows AMCL to run from bag file
|
||||
#include <rosbag/bag.h>
|
||||
#include <rosbag/view.h>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
// For monitoring the estimator
|
||||
#include <diagnostic_updater/diagnostic_updater.h>
|
||||
|
||||
// using namespace amcl;
|
||||
|
||||
#define NEW_UNIFORM_SAMPLING 1
|
||||
|
||||
// Pose hypothesis
|
||||
typedef struct
|
||||
{
|
||||
// Total weight (weights sum to 1)
|
||||
double weight;
|
||||
|
||||
// Mean of pose esimate
|
||||
pf_vector_t pf_pose_mean;
|
||||
|
||||
// Covariance of pose estimate
|
||||
pf_matrix_t pf_pose_cov;
|
||||
|
||||
} amcl_hyp_t;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/* This function is only useful to have the whole code work
|
||||
* with old rosbags that have trailing slashes for their frames
|
||||
*/
|
||||
inline
|
||||
std::string stripSlash(const std::string& in)
|
||||
{
|
||||
std::string out = in;
|
||||
if ((!in.empty()) && (in[0] == '/'))
|
||||
out.erase(0, 1);
|
||||
return out;
|
||||
}
|
||||
namespace amcl
|
||||
{
|
||||
class Amcl
|
||||
{
|
||||
public:
|
||||
Amcl();
|
||||
Amcl(ros::NodeHandle nh);
|
||||
virtual ~Amcl();
|
||||
|
||||
/**
|
||||
* @brief Uses TF and LaserScan messages from bag file to drive AMCL instead
|
||||
* @param in_bag_fn input bagfile
|
||||
* @param trigger_global_localization whether to trigger global localization
|
||||
* before starting to process the bagfile
|
||||
*/
|
||||
void runFromBag(const std::string& in_bag_fn, bool trigger_global_localization = false);
|
||||
|
||||
int process();
|
||||
void savePoseToServer(geometry_msgs::PoseWithCovarianceStamped &amcl_pose);
|
||||
bool getInitialized() {return initialized_;}
|
||||
|
||||
private:
|
||||
bool initialized_;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tfl_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
|
||||
bool sent_first_transform_;
|
||||
|
||||
tf2::Transform latest_tf_;
|
||||
bool latest_tf_valid_;
|
||||
|
||||
// Pose-generating function used to uniformly distribute particles over
|
||||
// the map
|
||||
static pf_vector_t uniformPoseGenerator(void* arg);
|
||||
#if NEW_UNIFORM_SAMPLING
|
||||
static std::vector<std::pair<int, int> > free_space_indices;
|
||||
#endif
|
||||
// Callbacks
|
||||
bool globalLocalizationCallback(std_srvs::Empty::Request& req,
|
||||
std_srvs::Empty::Response& res);
|
||||
bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
|
||||
std_srvs::Empty::Response& res);
|
||||
bool setMapCallback(nav_msgs::SetMap::Request& req,
|
||||
nav_msgs::SetMap::Response& res);
|
||||
|
||||
void initialize(ros::NodeHandle nh);
|
||||
void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);
|
||||
void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
|
||||
void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);
|
||||
void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);
|
||||
|
||||
void handleMapMessage(const nav_msgs::OccupancyGrid& msg);
|
||||
void freeMapDependentMemory();
|
||||
map_t* convertMap(const nav_msgs::OccupancyGrid& map_msg);
|
||||
void updatePoseFromServer();
|
||||
void applyInitialPose();
|
||||
|
||||
//parameter for which odom to use
|
||||
std::string odom_frame_id_;
|
||||
|
||||
//paramater to store latest odom pose
|
||||
geometry_msgs::PoseStamped latest_odom_pose_;
|
||||
|
||||
//parameter for which base to use
|
||||
std::string base_frame_id_;
|
||||
std::string global_frame_id_;
|
||||
|
||||
bool use_map_topic_;
|
||||
bool first_map_only_;
|
||||
|
||||
ros::Duration gui_publish_period;
|
||||
ros::Time save_pose_last_time;
|
||||
ros::Duration save_pose_period;
|
||||
|
||||
geometry_msgs::PoseWithCovarianceStamped last_published_pose;
|
||||
|
||||
map_t* map_;
|
||||
char* mapdata;
|
||||
int sx, sy;
|
||||
double resolution;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
|
||||
tf2_ros::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
|
||||
ros::Subscriber initial_pose_sub_;
|
||||
std::vector< amcl::AMCLLaser* > lasers_;
|
||||
std::vector< bool > lasers_update_;
|
||||
std::map< std::string, int > frame_to_laser_;
|
||||
|
||||
// Particle filter
|
||||
pf_t* pf_;
|
||||
double pf_err_, pf_z_;
|
||||
bool pf_init_;
|
||||
pf_vector_t pf_odom_pose_;
|
||||
double d_thresh_, a_thresh_;
|
||||
int resample_interval_;
|
||||
int resample_count_;
|
||||
double laser_min_range_;
|
||||
double laser_max_range_;
|
||||
|
||||
//Nomotion update control
|
||||
bool m_force_update; // used to temporarily let amcl update samples even when no motion occurs...
|
||||
|
||||
amcl::AMCLOdom* odom_;
|
||||
amcl::AMCLLaser* laser_;
|
||||
|
||||
ros::Duration cloud_pub_interval;
|
||||
ros::Time last_cloud_pub_time;
|
||||
|
||||
// For slowing play-back when reading directly from a bag file
|
||||
ros::WallDuration bag_scan_period_;
|
||||
|
||||
void requestMap();
|
||||
|
||||
// Helper to get odometric pose from transform system
|
||||
bool getOdomPose(geometry_msgs::PoseStamped& pose,
|
||||
double& x, double& y, double& yaw,
|
||||
const ros::Time& t, const std::string& f);
|
||||
|
||||
//time for tolerance on the published transform,
|
||||
//basically defines how long a map->odom transform is good for
|
||||
ros::Duration transform_tolerance_;
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle private_nh_;
|
||||
ros::Publisher pose_pub_;
|
||||
ros::Publisher particlecloud_pub_;
|
||||
ros::ServiceServer global_loc_srv_;
|
||||
ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
|
||||
ros::ServiceServer set_map_srv_;
|
||||
ros::Subscriber initial_pose_sub_old_;
|
||||
ros::Subscriber map_sub_;
|
||||
|
||||
diagnostic_updater::Updater diagnosic_updater_;
|
||||
void standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status);
|
||||
double std_warn_level_x_;
|
||||
double std_warn_level_y_;
|
||||
double std_warn_level_yaw_;
|
||||
|
||||
std::string scan_topic_;
|
||||
std::string map_topic_;
|
||||
std::string map_service_;
|
||||
|
||||
amcl_hyp_t* initial_pose_hyp_;
|
||||
bool first_map_received_;
|
||||
bool first_reconfigure_call_;
|
||||
|
||||
boost::recursive_mutex configuration_mutex_;
|
||||
dynamic_reconfigure::Server<amcl::AMCLConfig>* dsrv_;
|
||||
amcl::AMCLConfig default_config_;
|
||||
ros::Timer check_laser_timer_;
|
||||
|
||||
int max_beams_, min_particles_, max_particles_;
|
||||
double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
|
||||
double alpha_slow_, alpha_fast_;
|
||||
double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
|
||||
//beam skip related params
|
||||
bool do_beamskip_;
|
||||
double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;
|
||||
double laser_likelihood_max_dist_;
|
||||
amcl::odom_model_t odom_model_type_;
|
||||
double init_pose_[3];
|
||||
double init_cov_[3];
|
||||
amcl::laser_model_t laser_model_type_;
|
||||
bool tf_broadcast_;
|
||||
bool force_update_after_initialpose_;
|
||||
bool force_update_after_set_map_;
|
||||
bool selective_resampling_;
|
||||
ros::Time last_tfb_time_;
|
||||
|
||||
void reconfigureCB(amcl::AMCLConfig& config, uint32_t level);
|
||||
|
||||
ros::Time last_laser_received_ts_;
|
||||
ros::Duration laser_check_interval_;
|
||||
void checkLaserReceived(const ros::TimerEvent& event);
|
||||
};
|
||||
} // namespace amcl
|
||||
#define USAGE "USAGE: amcl"
|
||||
|
||||
#if NEW_UNIFORM_SAMPLING
|
||||
std::vector<std::pair<int, int> > amcl::Amcl::free_space_indices;
|
||||
#endif
|
||||
|
||||
#endif // _AMCL_LIBRARY_H_INCLUDED_
|
||||
150
Localizations/Libraries/Ros/amcl/include/amcl/map/map.h
Normal file
150
Localizations/Libraries/Ros/amcl/include/amcl/map/map.h
Normal 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
Localizations/Libraries/Ros/amcl/include/amcl/pf/eig3.h
Normal file
11
Localizations/Libraries/Ros/amcl/include/amcl/pf/eig3.h
Normal 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
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf.h
Normal file
210
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf.h
Normal 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
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_kdtree.h
Normal file
109
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_kdtree.h
Normal 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
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_pdf.h
Normal file
85
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_pdf.h
Normal 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
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_vector.h
Normal file
94
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_vector.h
Normal 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
|
||||
@@ -0,0 +1,35 @@
|
||||
#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.
|
||||
|
||||
// Remove extern declarations to avoid conflicts
|
||||
// extern double drand48(void); // Change this to static if it should be static
|
||||
|
||||
extern double drand48(void)
|
||||
{
|
||||
return ((double)rand())/RAND_MAX;
|
||||
}
|
||||
|
||||
// Remove extern declarations to avoid conflicts
|
||||
// extern double srand48(long int seedval); // Change this to static if it should be static
|
||||
|
||||
extern void srand48(long int seedval)
|
||||
{
|
||||
srand(seedval);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
Localizations/Libraries/Ros/amcl/package.xml
Normal file
47
Localizations/Libraries/Ros/amcl/package.xml
Normal 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>
|
||||
1409
Localizations/Libraries/Ros/amcl/src/amcl.cpp
Normal file
1409
Localizations/Libraries/Ros/amcl/src/amcl.cpp
Normal file
File diff suppressed because it is too large
Load Diff
84
Localizations/Libraries/Ros/amcl/src/amcl/map/map.c
Normal file
84
Localizations/Libraries/Ros/amcl/src/amcl/map/map.c
Normal 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;
|
||||
}
|
||||
|
||||
177
Localizations/Libraries/Ros/amcl/src/amcl/map/map_cspace.cpp
Normal file
177
Localizations/Libraries/Ros/amcl/src/amcl/map/map_cspace.cpp
Normal file
@@ -0,0 +1,177 @@
|
||||
/*
|
||||
* 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"
|
||||
#include <ros/ros.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
Localizations/Libraries/Ros/amcl/src/amcl/map/map_draw.c
Normal file
158
Localizations/Libraries/Ros/amcl/src/amcl/map/map_draw.c
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/map/map_range.c
Normal file
120
Localizations/Libraries/Ros/amcl/src/amcl/map/map_range.c
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/map/map_store.c
Normal file
215
Localizations/Libraries/Ros/amcl/src/amcl/map/map_store.c
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/pf/eig3.c
Normal file
274
Localizations/Libraries/Ros/amcl/src/amcl/pf/eig3.c
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf.c
Normal file
763
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf.c
Normal 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 "amcl/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
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_draw.c
Normal file
163
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_draw.c
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_kdtree.c
Normal file
486
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_kdtree.c
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_pdf.c
Normal file
147
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_pdf.c
Normal 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 "amcl/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
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_vector.c
Normal file
276
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_vector.c
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_laser.cpp
Normal file
510
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_laser.cpp
Normal 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
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_odom.cpp
Normal file
310
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_odom.cpp
Normal 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;
|
||||
}
|
||||
@@ -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
|
||||
72
Localizations/Libraries/Ros/amcl/src/amcl_node.cpp
Normal file
72
Localizations/Libraries/Ros/amcl/src/amcl_node.cpp
Normal file
@@ -0,0 +1,72 @@
|
||||
/*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* 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
|
||||
*
|
||||
*/
|
||||
|
||||
/* Author: Brian Gerkey */
|
||||
|
||||
// Signal handling
|
||||
#include <signal.h>
|
||||
#include <amcl/amcl.h>
|
||||
|
||||
|
||||
boost::shared_ptr<amcl::Amcl> amcl_node_ptr;
|
||||
|
||||
void sigintHandler(int sig)
|
||||
{
|
||||
// Save latest pose as we're shutting down.
|
||||
geometry_msgs::PoseWithCovarianceStamped amcl_pose;
|
||||
amcl_node_ptr->savePoseToServer(amcl_pose);
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "amcl");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// Override default sigint handler
|
||||
signal(SIGINT, sigintHandler);
|
||||
|
||||
// Make our node available to sigintHandler
|
||||
amcl_node_ptr.reset(new amcl::Amcl());
|
||||
|
||||
if (argc == 1)
|
||||
{
|
||||
// run using ROS input
|
||||
ros::spin();
|
||||
}
|
||||
else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag"))
|
||||
{
|
||||
if (argc == 3)
|
||||
{
|
||||
amcl_node_ptr->runFromBag(argv[2]);
|
||||
}
|
||||
else if ((argc == 4) && (std::string(argv[3]) == "--global-localization"))
|
||||
{
|
||||
amcl_node_ptr->runFromBag(argv[2], true);
|
||||
}
|
||||
}
|
||||
|
||||
// Without this, our boost locks are not shut down nicely
|
||||
amcl_node_ptr.reset();
|
||||
|
||||
// To quote Morgan, Hooray!
|
||||
return(0);
|
||||
}
|
||||
|
||||
101
Localizations/Libraries/Ros/amcl/test/basic_localization.py
Normal file
101
Localizations/Libraries/Ros/amcl/test/basic_localization.py
Normal 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)
|
||||
@@ -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>
|
||||
@@ -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
Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml
Normal file
51
Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml
Normal 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
Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml
Normal file
54
Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml
Normal 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>
|
||||
@@ -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
Localizations/Libraries/Ros/amcl/test/set_pose.py
Normal file
46
Localizations/Libraries/Ros/amcl/test/set_pose.py
Normal 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()
|
||||
@@ -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>
|
||||
@@ -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
Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml
Normal file
48
Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml
Normal 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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
Reference in New Issue
Block a user