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

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/bug_report.md vendored Normal file
View File

@@ -0,0 +1,30 @@
---
name: Bug report
about: For filing confirmed bugs
title: ''
labels: bug
assignees: ayrton04
---
<!-- All questions should be directed to answers.ros.org. Similarly, before filing a bug report, please check for solutions to your issue on answers.ros.org. -->
## Bug report
**Required Info:**
- Operating System:
- <!-- OS and version (e.g. Ubuntu 16.04) -->
- Installation type:
- <!-- binaries or from source -->
- Version or commit hash:
- <!-- Output of git rev-parse HEAD, release version, or repos file -->
#### Steps to reproduce issue
<!-- Detailed instructions on how to reliably reproduce this issue -->
#### Expected behavior
#### Actual behavior
#### Additional information

16
Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/feature_request.md vendored Normal file
View File

@@ -0,0 +1,16 @@
---
name: Feature request
about: For requesting new features
title: ''
labels: feature request
assignees: ayrton04
---
## Feature request
#### Feature description
<!-- Description in a few sentences what the feature consists of and what problem it will solve -->
#### Implementation considerations
<!-- Relevant information on how the feature could be implemented and pros and cons of the different solutions -->

4
Localizations/Packages/robot_localization/.gitignore vendored Normal file
View File

@@ -0,0 +1,4 @@
doc/html
*.cproject
*.project
*.settings

311
Localizations/Packages/robot_localization/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,311 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package robot_localization
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2.7.4 (2022-07-27)
------------------
* Fix odometry and acceleration processing pipeline (`#753 <https://github.com/cra-ros-pkg/robot_localization/issues/753>`_)
* Use default CXX, need to specify boost::placeholders::_1 instead of just _1 (`#750 <https://github.com/cra-ros-pkg/robot_localization/issues/750>`_)
* Fix odometry msgs with child frame other than baseLink (`#728 <https://github.com/cra-ros-pkg/robot_localization/issues/728>`_)
* update documentation (`#723 <https://github.com/cra-ros-pkg/robot_localization/issues/723>`_)
* Fix unused-parameter warning (`#721 <https://github.com/cra-ros-pkg/robot_localization/issues/721>`_)
* Fix tf lookup timestamp during map->odom publication (`#719 <https://github.com/cra-ros-pkg/robot_localization/issues/719>`_)
* UKF cleanup (`#671 <https://github.com/cra-ros-pkg/robot_localization/issues/671>`_)
* Added geographiclib to catkin exported depends (`#709 <https://github.com/cra-ros-pkg/robot_localization/issues/709>`_)
* Make the navsat tf frame name parametric (`#699 <https://github.com/cra-ros-pkg/robot_localization/issues/699>`_)
* Propagate the suppression of tf warnings (`#705 <https://github.com/cra-ros-pkg/robot_localization/issues/705>`_)
* Fix typo in base_link_frame_output name. (`#701 <https://github.com/cra-ros-pkg/robot_localization/issues/701>`_)
* Contributors: AR Dabbour, Carlos Agüero, Haoguang Yang, J.P.S, Joshua Owen, Leonardo Hemerly, Lucas Walter, Marcus Scheunemann, Stephen Williams, Tom Moore
2.7.3 (2021-07-23)
------------------
* Prevent node from crashing on invalid UTM zone, but throw ROS_ERROR to notify user (`#682 <https://github.com/cra-ros-pkg/robot_localization/issues/682>`_)
* changed geographiclib to <depend> tag (`#684 <https://github.com/cra-ros-pkg/robot_localization/issues/684>`_)
* Small formatting and content change (`#677 <https://github.com/cra-ros-pkg/robot_localization/issues/677>`_)
* Fixed state transition for sigma points. The state transition function for each sigma point now uses its sigma point's posteriori state instead of always the posteriori state. (`#628 <https://github.com/cra-ros-pkg/robot_localization/issues/628>`_)
Co-authored-by: jola6897 <jola6897@users.noreply.github.com>
* Contributors: John Stechschulte, Jonas, MCFurry, Vivek Mhatre
2.7.2 (2021-06-03)
------------------
* Also test for gamma conversion (`#647 <https://github.com/cra-ros-pkg/robot_localization/issues/647>`_)
* fix: Transform gravitation vector to IMU frame before removing acceleration (`#639 <https://github.com/cra-ros-pkg/robot_localization/issues/639>`_)
* Fixed a typo in validate filter output error message. (`#646 <https://github.com/cra-ros-pkg/robot_localization/issues/646>`_)
* Stick to the global utm_zone\_ when transforming gps to UTM (`#627 <https://github.com/cra-ros-pkg/robot_localization/issues/627>`_)
* Stick to the global utm_zone\_ when transforming gps to UTM
* UTM conversions using geographiclib (`#626 <https://github.com/cra-ros-pkg/robot_localization/issues/626>`_)
* Use GeographicLib for UTMtoLL conversions
* SHARED linking for Geographiclib (`#624 <https://github.com/cra-ros-pkg/robot_localization/issues/624>`_)
* remove GeographicLib specific linking option
* Fixing lat-long to UTM conversion (`#620 <https://github.com/cra-ros-pkg/robot_localization/issues/620>`_)
Thanks again for the report. I'll get this into `noetic-devel` and the relevant ROS2 branches.
* Removing xmlrpcpp dependency
* yaml-cpp using find_package as backup (`#618 <https://github.com/cra-ros-pkg/robot_localization/issues/618>`_)
* Add ${GeographicLib_LIBRARIES} to navsat_transform (`#617 <https://github.com/cra-ros-pkg/robot_localization/issues/617>`_)
* Contributors: Achmad Fathoni, Leonardo Hemerly, Paul Verhoeckx, Tim Clephas, Tobias Fischer, Tom Moore
2.7.0 (2020-12-17)
------------------
* Making repeated state publication optional (`#595 <https://github.com/cra-ros-pkg/robot_localization/issues/595>`_)
* Fix sign error in dFY_dP part of transferFunctionJacobian\_ (`#592 <https://github.com/cra-ros-pkg/robot_localization/issues/592>`_)
* Fix typo in navsat_transform_node.rst (`#588 <https://github.com/cra-ros-pkg/robot_localization/issues/588>`_)
* Fix issue caused by starting on uneven terrain (`#582 <https://github.com/cra-ros-pkg/robot_localization/issues/582>`_)
* Local Cartesian Option (`#575 <https://github.com/cra-ros-pkg/robot_localization/issues/575>`_)
* Fix frame id of imu in differential mode, closes `#482 <https://github.com/cra-ros-pkg/robot_localization/issues/482>`_. (`#522 <https://github.com/cra-ros-pkg/robot_localization/issues/522>`_)
* navsat_transform diagram to address `#550 <https://github.com/cra-ros-pkg/robot_localization/issues/550>`_ (`#570 <https://github.com/cra-ros-pkg/robot_localization/issues/570>`_)
* Increasing the minimum CMake version (`#573 <https://github.com/cra-ros-pkg/robot_localization/issues/573>`_)
* Contributors: Aleksander Bojda, David Jensen, James Baxter, Jeffrey Kane Johnson, Mabel Zhang, Ronald Ensing, Tom Moore
2.6.8 (2020-06-03)
------------------
* Adding conditional build dependencies (`#572 <https://github.com/cra-ros-pkg/robot_localization/issues/572>`_)
* Contributors: Tom Moore
2.6.7 (2020-06-01)
------------------
* Parameterizing transform failure warnings
* [melodic] Fix Windows build break. (`#557 <https://github.com/cra-ros-pkg/robot_localization/issues/557>`_)
* Contributors: Sean Yen, Tom Moore, florianspy
2.6.5 (2019-08-08)
------------------
* fix: wall time used when `use_sim_time` is true
* Created service for converting to / from lat long
* Fix bug with tf_prefix
* Adding new contribution to doc
* Add missing undocumented params
* Update wiki location
* Contributors: Andrew Grindstaff, Axel Mousset, Charles Brian Quinn, Oswin So, Tom Moore
2.6.4 (2019-02-15)
------------------
* Meridian convergence adjustment added to navsat_transform.
* Documentation changes
* Add broadcast_utm_transform_as_parent_frame
* Enable build optimisations if no build type configured.
* Contributors: G.A. vd. Hoorn, Pavlo Kolomiiets, diasdm
2.6.3 (2019-01-14)
------------------
* Rename odomBaseLinkTrans to baseLinkOdomTrans
Adhere to the naming convention <fromFrame><toFrame>Trans used for worldBaseLinkTrans and mapOdomTrans.
* Add const& to catch values to prevent the error: catching polymorphic type ‘class tf2::TransformException’ by value
And ZoneNumber by 0x3fU to prevent error: directive output may be truncated writing between 1 and 11 bytes into a region of size 4
* Enabling the user to override the output child_frame_id
* Fixing Euler body-to-world transformations
* Whitespace
* fixing no datum service in melodic
* Contributors: Alexis schad, Matthew Jones, Tom Moore, thallerod
2.6.2 (2018-10-25)
------------------
* Fixing tests
* Contributors: Tom Moore
2.6.1 (2018-10-25)
------------------
* Adding more output for measurement history failures
* Adding filter processing toggle service
* Waiting for valid ROS time before starting navsat_transform_node
* Contributors: Tom Moore, stevemacenski
2.6.0 (2018-07-27)
------------------
* Moving to C++14, adding error flags, and fixing all warnings
* Contributors: Tom Moore
2.5.2 (2018-04-11)
------------------
* Add published accel topic to documentation
* adding log statements for nans in the invertable matrix
* Fixing issue with potential seg fault
* Contributors: Oleg Kalachev, Tom Moore, stevemacenski
2.5.1 (2018-01-03)
------------------
* Fixing CMakeLists
* Contributors: Tom Moore
2.5.0 (2017-12-15)
------------------
* Fixing datum precision
* Fixing timing variable
* Fixing state history reversion
* Fixing critical bug with dynamic process noise covariance
* Fix typo in reading Mahalanobis thresholds.
* Zero out rotation in GPS to base_link transform
* Update xmlrpcpp includes for Indigo support
* Removing lastUpdateTime
* Fixing timestamps in map->odom transform
* Simplify enabledAtStartup logic
* Add std_srvs dependency
* Add enabling service
* Ensure all raw sensor input orientations are normalized even if messages are not
* Install params directory.
* Add robot localization estimator
* Adding nodelet support
* Contributors: Jacob Perron, Jacob Seibert, Jiri Hubacek, Mike Purvis, Miquel Massot, Pavlo Kolomiiets, Rein Appeldoorn, Rokus Ottervanger, Simon Gene Gottlieb, Tom Moore, stevemacenski
2.4.0 (2017-06-12)
------------------
* Updated documentation
* Added reset_on_time_jump option
* Added feature to optionally publish utm frame as parent in navsat_transform_node
* Moved global callback queue reset
* Added initial_state parameter and documentation
* Fixed ac/deceleration gains default logic
* Added gravity parameter
* Added delay and throttle if tf lookup fails
* Fixed UKF IMUTwistBasicIO test
* Added transform_timeout parameter
* Set gps_odom timestamp before tf2 lookuptransform
* Removed non-portable sincos calls
* Simplified logic to account for correlated error
* Added dynamic process noise covariance calculation
* Fixed catkin_package Eigen warning
* Added optional publication of acceleration state
* Contributors: Brian Gerkey, Enrique Fernandez, Jochen Sprickerhof, Rein Appeldoorn, Simon Gene Gottlieb, Tom Moore
2.3.1 (2016-10-27)
------------------
* Adding gitignore
* Adding remaining wiki pages
* Adding config and prep pages
* Adding navsat_transform_node documentation
* use_odometry_yaw fix for n_t_n
* Fixing issue with manual pose reset when history is not empty
* Getting inverse transform when looking up robot's pose.
* Sphinx documentation
* Removing forward slashes from navsat_transform input topics for template launch file
* Adding example launch and parameter files for a two-level EKF setup with navsat_transform_node
* Adding yaml file for navsat_transform_node, and moving parameter documentation to it.
* Updating EKF and UKF parameter templates with usage comments
* Contributors: Tom Moore, asimay
2.3.0 (2016-07-28)
------------------
* Fixed issues with datum usage and frame_ids
* Fixed comment for wait_for_datum
* Fixing issue with non-zero navsat sensor orientation offsets
* Fixing issue with base_link->gps transform wrecking the 'true' UTM position computation
* Using correct covariance for filtered GPS
* Fixed unitialized odometry covariance bug
* Added filter history and measurement queue behavior
* Changing output timestamp to more accurately use the time stamp of the most recently-processed measurement
* Added TcpNoDelay()
* Added parameter to make transform publishing optional
* Fixed differential handling for pose data so that it doesn't care about the message's frame_id
* Updated UKF config and launch
* Added a test case for the timestamp diagnostics
* Added reporting of bad timestamps via diagnostics
* Updated tests to match new method signatures
* Added control term
* Added smoothing capability for delayed measurements
* Making variables in navsat_transform conform to ROS coding standards
* Contributors: Adel Fakih, Ivor Wanders, Marc Essinger, Tobias Tueylue, Tom Moore
2.2.3 (2016-04-24)
------------------
* Cleaning up callback data structure and callbacks and updating doxygen comments in headers
* Removing MessageFilters
* Removing deprecated parameters
* Adding the ability to handle GPS offsets from the vehicle's origin
* Cleaning up navsat_transform.h
* Making variables in navsat_transform conform to ROS coding standards
2.2.2 (2016-02-04)
------------------
* Updating trig functions to use sincos for efficiency
* Updating licensing information and adding Eigen MPL-only flag
* Added state to imu frame transformation
* Using state orientation if imu orientation is missing
* Manually adding second spin for odometry and IMU data that is passed to message filters
* Reducing delay between measurement reception and filter output
* Zero altitute in intital transform too, when zero altitude param is set
* Fixing regression with conversion back to GPS coordinates
* Switched cropping of orientation data in inovationSubset with mahalanobis check to prevent excluding measurements with orientations bigger/smaller than ± PI
* Fix Jacobian for EKF.
* Removing warning about orientation variables when only their velocities are measured
* Checking for -1 in IMU covariances and ignoring relevant message data
* roslint and catkin_lint applied
* Adding base_link to datum specification, and fixing bug with order of measurement handling when a datum is specified. Also added check to make sure IMU data is transformable before using it.
* Contributors: Adnan Ademovic, Jit Ray Chowdhury, Philipp Tscholl, Tom Moore, ayrton04, kphil
2.2.1 (2015-05-27)
------------------
* Fixed handling of IMU data w.r.t. differential mode and relative mode
2.2.0 (2015-05-22)
------------------
* Added tf2-friendly tf_prefix appending
* Corrected for IMU orientation in navsat_transform
* Fixed issue with out-of-order measurements and pose resets
* Nodes now assume ENU standard for yaw data
* Removed gps_common dependency
* Adding option to navsat_transform_node that enables the use of the heading from the odometry message instead of an IMU.
* Changed frame_id used in setPoseCallback to be the world_frame
* Optimized Eigen arithmetic for signficiant performance boost
* Migrated to tf2
* Code refactoring and reorganization
* Removed roll and pitch from navsat_transform calculations
* Fixed transform for IMU data to better support mounting IMUs in non-standard orientations
* Added feature to navsat_transform_node whereby filtered odometry data can be coverted back into navsat data
* Added a parameter to allow future dating the world_frame->base_link_frame transform.
* Removed deprecated differential setting handler
* Added relative mode
* Updated and improved tests
* Fixing source frame_id in pose data handling
* Added initial covariance parameter
* Fixed bug in covariance copyinh
* Added parameters for topic queue sizes
* Improved motion model's handling of angular velocities when robot has non-zero roll and pitch
* Changed the way differential measurements are handled
* Added diagnostics
2.1.7 (2015-01-05)
------------------
* Added some checks to eliminate unnecessary callbacks
* Updated launch file templates
* Added measurement outlier rejection
* Added failure callbacks for tf message filters
* Added optional broadcast of world_frame->utm transform for navsat_transform_node
* Bug fixes for differential mode and handling of Z acceleration in 2D mode
2.1.6 (2014-11-06)
------------------
* Added unscented Kalman filter (UKF) localization node
* Fixed map->odom tf calculation
* Acceleration data from IMUs is now used in computing the state estimate
* Added 2D mode
2.1.5 (2014-10-07)
------------------
* Changed initial estimate error covariance to be much smaller
* Fixed some debug output
* Added test suite
* Better compliance with REP-105
* Fixed differential measurement handling
* Implemented message filters
* Added navsat_transform_node
2.1.4 (2014-08-22)
------------------
* Adding utm_transform_node to install targets
2.1.3 (2014-06-22)
------------------
* Some changes to ease GPS integration
* Addition of differential integration of pose data
* Some documentation cleanup
* Added UTM transform node and launch file
* Bug fixes
2.1.2 (2014-04-11)
------------------
* Updated covariance correction formulation to "Joseph form" to improve filter stability.
* Implemented new versioning scheme.
2.1.1 (2014-04-11)
------------------
* Added cmake_modules dependency for Eigen support, and added include to silence boost::signals warning from tf include

354
Localizations/Packages/robot_localization/CMakeLists.txt Normal file
View File

@@ -0,0 +1,354 @@
cmake_minimum_required(VERSION 3.0.2)
project(robot_localization)
if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.")
set(CMAKE_BUILD_TYPE RelWithDebInfo)
endif()
find_package(catkin REQUIRED COMPONENTS
angles
diagnostic_msgs
diagnostic_updater
eigen_conversions
geographic_msgs
geometry_msgs
message_filters
message_generation
nav_msgs
nodelet
roscpp
roslint
sensor_msgs
std_msgs
std_srvs
tf2
tf2_geometry_msgs
tf2_ros)
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML_CPP yaml-cpp)
if(NOT YAML_CPP_FOUND)
find_package(yaml-cpp REQUIRED)
endif()
# Geographiclib installs FindGeographicLib.cmake to this non-standard location
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/")
find_package(GeographicLib REQUIRED)
# Attempt to find Eigen using its own CMake module.
# If that fails, fall back to cmake_modules package.
find_package(Eigen3)
set(EIGEN_PACKAGE EIGEN3)
if(NOT EIGEN3_FOUND)
find_package(cmake_modules REQUIRED)
find_package(Eigen REQUIRED)
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})
set(EIGEN_PACKAGE Eigen)
endif()
if(NOT MSVC)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror")
endif()
add_definitions(-DEIGEN_NO_DEBUG -DEIGEN_MPL2_ONLY)
set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references")
roslint_cpp()
###################################
## catkin specific configuration ##
###################################
add_service_files(
FILES
GetState.srv
SetDatum.srv
SetPose.srv
SetUTMZone.srv
ToggleFilterProcessing.srv
FromLL.srv
ToLL.srv
)
generate_messages(
DEPENDENCIES
geographic_msgs
geometry_msgs
std_msgs
)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
ekf
ekf_localization_nodelet
filter_base
filter_utilities
navsat_transform
navsat_transform_nodelet
ros_filter
ros_filter_utilities
robot_localization_estimator
ros_robot_localization_listener
ukf
ukf_localization_nodelet
CATKIN_DEPENDS
angles
cmake_modules
diagnostic_msgs
diagnostic_updater
eigen_conversions
geographic_msgs
geometry_msgs
message_filters
message_runtime
nav_msgs
roscpp
sensor_msgs
std_msgs
std_srvs
tf2
tf2_geometry_msgs
tf2_ros
DEPENDS
${EIGEN_PACKAGE}
GeographicLib
YAML_CPP
)
###########
## Build ##
###########
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${YAML_CPP_INCLUDE_DIRS})
link_directories(${YAML_CPP_LIBRARY_DIRS})
# Library definitions
add_library(filter_utilities src/filter_utilities.cpp)
add_library(filter_base src/filter_base.cpp)
add_library(ekf src/ekf.cpp)
add_library(ukf src/ukf.cpp)
add_library(robot_localization_estimator src/robot_localization_estimator.cpp)
add_library(ros_robot_localization_listener src/ros_robot_localization_listener.cpp)
add_library(ros_filter_utilities src/ros_filter_utilities.cpp)
add_library(ros_filter src/ros_filter.cpp)
add_library(navsat_transform src/navsat_transform.cpp)
add_library(ekf_localization_nodelet src/ekf_localization_nodelet.cpp)
add_library(ukf_localization_nodelet src/ukf_localization_nodelet.cpp)
add_library(navsat_transform_nodelet src/navsat_transform_nodelet.cpp)
# Executables
add_executable(ekf_localization_node src/ekf_localization_node.cpp)
add_executable(ukf_localization_node src/ukf_localization_node.cpp)
add_executable(navsat_transform_node src/navsat_transform_node.cpp)
add_executable(robot_localization_listener_node src/robot_localization_listener_node.cpp)
# Dependencies
add_dependencies(filter_base ${PROJECT_NAME}_gencpp)
add_dependencies(navsat_transform ${PROJECT_NAME}_gencpp)
add_dependencies(robot_localization_listener_node ${PROJECT_NAME}_gencpp)
# Linking
target_link_libraries(ros_filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
target_link_libraries(filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
target_link_libraries(filter_base filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
target_link_libraries(ekf filter_base ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
target_link_libraries(ukf filter_base ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
target_link_libraries(ros_filter ekf ukf ros_filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
target_link_libraries(robot_localization_estimator filter_utilities filter_base ekf ukf ${EIGEN3_LIBRARIES})
target_link_libraries(ros_robot_localization_listener robot_localization_estimator ros_filter_utilities
${catkin_LIBRARIES} ${EIGEN3_LIBRARIES} ${YAML_CPP_LIBRARIES})
target_link_libraries(robot_localization_listener_node ros_robot_localization_listener ${catkin_LIBRARIES})
target_link_libraries(ekf_localization_node ros_filter ${catkin_LIBRARIES})
target_link_libraries(ekf_localization_nodelet ros_filter ${catkin_LIBRARIES})
target_link_libraries(ukf_localization_node ros_filter ${catkin_LIBRARIES})
target_link_libraries(ukf_localization_nodelet ros_filter ${catkin_LIBRARIES})
target_link_libraries(navsat_transform filter_utilities ros_filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES} ${GeographicLib_LIBRARIES})
target_link_libraries(navsat_transform_node navsat_transform ${catkin_LIBRARIES} ${GeographicLib_LIBRARIES})
target_link_libraries(navsat_transform_nodelet navsat_transform ${catkin_LIBRARIES} ${GeographicLib_LIBRARIES})
#############
## Install ##
#############
## Mark executables and/or libraries for installation
install(TARGETS
ekf
ekf_localization_nodelet
filter_base
filter_utilities
navsat_transform
navsat_transform_nodelet
ros_filter
ros_filter_utilities
robot_localization_estimator
ros_robot_localization_listener
ukf
ukf_localization_nodelet
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(TARGETS
ekf_localization_node
navsat_transform_node
robot_localization_listener_node
ukf_localization_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
FILES_MATCHING PATTERN "*.launch")
install(DIRECTORY params/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/params)
install(FILES
LICENSE
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES nodelet_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
if (CATKIN_ENABLE_TESTING)
roslint_add_test()
# Not really necessary, but it will cause the build to fail if it's
# missing, rather than failing once the tests are being executed
find_package(rosbag REQUIRED)
find_package(rostest REQUIRED)
#### FILTER BASE TESTS ####
catkin_add_gtest(filter_base-test test/test_filter_base.cpp)
target_link_libraries(filter_base-test filter_base ${catkin_LIBRARIES})
# This test uses ekf_localization node for convenience.
add_rostest_gtest(test_filter_base_diagnostics_timestamps
test/test_filter_base_diagnostics_timestamps.test
test/test_filter_base_diagnostics_timestamps.cpp)
target_link_libraries(test_filter_base_diagnostics_timestamps ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_filter_base_diagnostics_timestamps ekf_localization_node)
#### EKF TESTS #####
add_rostest_gtest(test_ekf
test/test_ekf.test
test/test_ekf.cpp)
target_link_libraries(test_ekf ros_filter ekf ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_rostest_gtest(test_ekf_localization_node_interfaces
test/test_ekf_localization_node_interfaces.test
test/test_ekf_localization_node_interfaces.cpp)
target_link_libraries(test_ekf_localization_node_interfaces ros_filter ekf ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ekf_localization_node_interfaces ekf_localization_node)
add_rostest_gtest(test_ekf_localization_node_bag1
test/test_ekf_localization_node_bag1.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ekf_localization_node_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ekf_localization_node_bag1 ekf_localization_node)
add_rostest_gtest(test_ekf_localization_node_bag2
test/test_ekf_localization_node_bag2.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ekf_localization_node_bag2 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ekf_localization_node_bag2 ekf_localization_node)
add_rostest_gtest(test_ekf_localization_node_bag3
test/test_ekf_localization_node_bag3.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ekf_localization_node_bag3 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ekf_localization_node_bag3 ekf_localization_node)
add_rostest_gtest(test_ekf_localization_nodelet_bag1
test/test_ekf_localization_nodelet_bag1.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ekf_localization_nodelet_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ekf_localization_nodelet_bag1 ekf_localization_nodelet)
#### UKF TESTS #####
add_rostest_gtest(test_ukf
test/test_ukf.test
test/test_ukf.cpp)
target_link_libraries(test_ukf ros_filter ukf ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_rostest_gtest(test_ukf_localization_node_interfaces
test/test_ukf_localization_node_interfaces.test
test/test_ukf_localization_node_interfaces.cpp)
target_link_libraries(test_ukf_localization_node_interfaces ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ukf_localization_node_interfaces ukf_localization_node)
add_rostest_gtest(test_ukf_localization_node_bag1
test/test_ukf_localization_node_bag1.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ukf_localization_node_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ukf_localization_node_bag1 ukf_localization_node)
add_rostest_gtest(test_ukf_localization_node_bag2
test/test_ukf_localization_node_bag2.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ukf_localization_node_bag2 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ukf_localization_node_bag2 ukf_localization_node)
add_rostest_gtest(test_ukf_localization_node_bag3
test/test_ukf_localization_node_bag3.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ukf_localization_node_bag3 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ukf_localization_node_bag3 ukf_localization_node)
add_rostest_gtest(test_ukf_localization_nodelet_bag1
test/test_ukf_localization_nodelet_bag1.test
test/test_localization_node_bag_pose_tester.cpp)
target_link_libraries(test_ukf_localization_nodelet_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES})
add_dependencies(test_ukf_localization_nodelet_bag1 ukf_localization_nodelet)
#### RLE/RLL TESTS #####
add_rostest_gtest(test_robot_localization_estimator
test/test_robot_localization_estimator.test
test/test_robot_localization_estimator.cpp)
target_link_libraries(test_robot_localization_estimator
robot_localization_estimator
${catkin_LIBRARIES}
${rostest_LIBRARIES})
add_executable(test_ros_robot_localization_listener_publisher test/test_ros_robot_localization_listener_publisher.cpp)
target_link_libraries(test_ros_robot_localization_listener_publisher
${catkin_LIBRARIES})
add_rostest_gtest(test_ros_robot_localization_listener
test/test_ros_robot_localization_listener.test
test/test_ros_robot_localization_listener.cpp)
target_link_libraries(test_ros_robot_localization_listener
ros_robot_localization_listener
${catkin_LIBRARIES}
${rostest_LIBRARIES})
#### NAVSAT CONVERSION TESTS ####
catkin_add_gtest(navsat_conversions-test test/test_navsat_conversions.cpp)
target_link_libraries(navsat_conversions-test navsat_transform ${catkin_LIBRARIES})
#### NAVSAT TRANSFORM TESTS ####
add_rostest_gtest(test_navsat_transform
test/test_navsat_transform.test
test/test_navsat_transform.cpp)
target_link_libraries(test_navsat_transform ${catkin_LIBRARIES} ${rostest_LIBRARIES})
endif()

68
Localizations/Packages/robot_localization/LICENSE Normal file
View File

@@ -0,0 +1,68 @@
All code within the robot_localization package that was developed by Charles
River Analytics is released under the BSD license:
Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the {organization} nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
==========================================================================
The robot_localization package also makes use of code written by The University
of Texas at Austin. This code is released under a modified BSD license:
Copyright (C) 2010 Austin Robot Technology, and others
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.
* Neither the names of the University of Texas at Austin, nor
Austin Robot Technology, nor the names of other contributors may
be used to endorse or promote products derived from this
software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

6
Localizations/Packages/robot_localization/README.md Normal file
View File

@@ -0,0 +1,6 @@
robot_localization
==================
robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc.
Please see documentation here: http://docs.ros.org/noetic/api/robot_localization/html/index.html

10
Localizations/Packages/robot_localization/doc/.templates/full_globaltoc.html Normal file
View File

@@ -0,0 +1,10 @@
<h3><a href="{{ pathto(master_doc) }}">{{ _('Table Of Contents') }}</a></h3>
{{ toctree(includehidden=True) }}
<h3><a href="{{ pathto(master_doc) }}">{{ _('Further Documentation') }}</a></h3>
<ul>
<li class="toctree-l1"><a href="api/index.html"/>API Documentation</a></li>
<li class="toctree-l1"><a href="https://github.com/cra-ros-pkg/robot_localization.git"/>GitHub Repository</a></li>
</ul>

311
Localizations/Packages/robot_localization/doc/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,311 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package robot_localization
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2.7.4 (2022-07-27)
------------------
* Fix odometry and acceleration processing pipeline (`#753 <https://github.com/cra-ros-pkg/robot_localization/issues/753>`_)
* Use default CXX, need to specify boost::placeholders::_1 instead of just _1 (`#750 <https://github.com/cra-ros-pkg/robot_localization/issues/750>`_)
* Fix odometry msgs with child frame other than baseLink (`#728 <https://github.com/cra-ros-pkg/robot_localization/issues/728>`_)
* update documentation (`#723 <https://github.com/cra-ros-pkg/robot_localization/issues/723>`_)
* Fix unused-parameter warning (`#721 <https://github.com/cra-ros-pkg/robot_localization/issues/721>`_)
* Fix tf lookup timestamp during map->odom publication (`#719 <https://github.com/cra-ros-pkg/robot_localization/issues/719>`_)
* UKF cleanup (`#671 <https://github.com/cra-ros-pkg/robot_localization/issues/671>`_)
* Added geographiclib to catkin exported depends (`#709 <https://github.com/cra-ros-pkg/robot_localization/issues/709>`_)
* Make the navsat tf frame name parametric (`#699 <https://github.com/cra-ros-pkg/robot_localization/issues/699>`_)
* Propagate the suppression of tf warnings (`#705 <https://github.com/cra-ros-pkg/robot_localization/issues/705>`_)
* Fix typo in base_link_frame_output name. (`#701 <https://github.com/cra-ros-pkg/robot_localization/issues/701>`_)
* Contributors: AR Dabbour, Carlos Agüero, Haoguang Yang, J.P.S, Joshua Owen, Leonardo Hemerly, Lucas Walter, Marcus Scheunemann, Stephen Williams, Tom Moore
2.7.3 (2021-07-23)
------------------
* Prevent node from crashing on invalid UTM zone, but throw ROS_ERROR to notify user (`#682 <https://github.com/cra-ros-pkg/robot_localization/issues/682>`_)
* changed geographiclib to <depend> tag (`#684 <https://github.com/cra-ros-pkg/robot_localization/issues/684>`_)
* Small formatting and content change (`#677 <https://github.com/cra-ros-pkg/robot_localization/issues/677>`_)
* Fixed state transition for sigma points. The state transition function for each sigma point now uses its sigma point's posteriori state instead of always the posteriori state. (`#628 <https://github.com/cra-ros-pkg/robot_localization/issues/628>`_)
Co-authored-by: jola6897 <jola6897@users.noreply.github.com>
* Contributors: John Stechschulte, Jonas, MCFurry, Vivek Mhatre
2.7.2 (2021-06-03)
------------------
* Also test for gamma conversion (`#647 <https://github.com/cra-ros-pkg/robot_localization/issues/647>`_)
* fix: Transform gravitation vector to IMU frame before removing acceleration (`#639 <https://github.com/cra-ros-pkg/robot_localization/issues/639>`_)
* Fixed a typo in validate filter output error message. (`#646 <https://github.com/cra-ros-pkg/robot_localization/issues/646>`_)
* Stick to the global utm_zone\_ when transforming gps to UTM (`#627 <https://github.com/cra-ros-pkg/robot_localization/issues/627>`_)
* Stick to the global utm_zone\_ when transforming gps to UTM
* UTM conversions using geographiclib (`#626 <https://github.com/cra-ros-pkg/robot_localization/issues/626>`_)
* Use GeographicLib for UTMtoLL conversions
* SHARED linking for Geographiclib (`#624 <https://github.com/cra-ros-pkg/robot_localization/issues/624>`_)
* remove GeographicLib specific linking option
* Fixing lat-long to UTM conversion (`#620 <https://github.com/cra-ros-pkg/robot_localization/issues/620>`_)
Thanks again for the report. I'll get this into `noetic-devel` and the relevant ROS2 branches.
* Removing xmlrpcpp dependency
* yaml-cpp using find_package as backup (`#618 <https://github.com/cra-ros-pkg/robot_localization/issues/618>`_)
* Add ${GeographicLib_LIBRARIES} to navsat_transform (`#617 <https://github.com/cra-ros-pkg/robot_localization/issues/617>`_)
* Contributors: Achmad Fathoni, Leonardo Hemerly, Paul Verhoeckx, Tim Clephas, Tobias Fischer, Tom Moore
2.7.0 (2020-12-17)
------------------
* Making repeated state publication optional (`#595 <https://github.com/cra-ros-pkg/robot_localization/issues/595>`_)
* Fix sign error in dFY_dP part of transferFunctionJacobian\_ (`#592 <https://github.com/cra-ros-pkg/robot_localization/issues/592>`_)
* Fix typo in navsat_transform_node.rst (`#588 <https://github.com/cra-ros-pkg/robot_localization/issues/588>`_)
* Fix issue caused by starting on uneven terrain (`#582 <https://github.com/cra-ros-pkg/robot_localization/issues/582>`_)
* Local Cartesian Option (`#575 <https://github.com/cra-ros-pkg/robot_localization/issues/575>`_)
* Fix frame id of imu in differential mode, closes `#482 <https://github.com/cra-ros-pkg/robot_localization/issues/482>`_. (`#522 <https://github.com/cra-ros-pkg/robot_localization/issues/522>`_)
* navsat_transform diagram to address `#550 <https://github.com/cra-ros-pkg/robot_localization/issues/550>`_ (`#570 <https://github.com/cra-ros-pkg/robot_localization/issues/570>`_)
* Increasing the minimum CMake version (`#573 <https://github.com/cra-ros-pkg/robot_localization/issues/573>`_)
* Contributors: Aleksander Bojda, David Jensen, James Baxter, Jeffrey Kane Johnson, Mabel Zhang, Ronald Ensing, Tom Moore
2.6.8 (2020-06-03)
------------------
* Adding conditional build dependencies (`#572 <https://github.com/cra-ros-pkg/robot_localization/issues/572>`_)
* Contributors: Tom Moore
2.6.7 (2020-06-01)
------------------
* Parameterizing transform failure warnings
* [melodic] Fix Windows build break. (`#557 <https://github.com/cra-ros-pkg/robot_localization/issues/557>`_)
* Contributors: Sean Yen, Tom Moore, florianspy
2.6.5 (2019-08-08)
------------------
* fix: wall time used when `use_sim_time` is true
* Created service for converting to / from lat long
* Fix bug with tf_prefix
* Adding new contribution to doc
* Add missing undocumented params
* Update wiki location
* Contributors: Andrew Grindstaff, Axel Mousset, Charles Brian Quinn, Oswin So, Tom Moore
2.6.4 (2019-02-15)
------------------
* Meridian convergence adjustment added to navsat_transform.
* Documentation changes
* Add broadcast_utm_transform_as_parent_frame
* Enable build optimisations if no build type configured.
* Contributors: G.A. vd. Hoorn, Pavlo Kolomiiets, diasdm
2.6.3 (2019-01-14)
------------------
* Rename odomBaseLinkTrans to baseLinkOdomTrans
Adhere to the naming convention <fromFrame><toFrame>Trans used for worldBaseLinkTrans and mapOdomTrans.
* Add const& to catch values to prevent the error: catching polymorphic type ‘class tf2::TransformException’ by value
And ZoneNumber by 0x3fU to prevent error: directive output may be truncated writing between 1 and 11 bytes into a region of size 4
* Enabling the user to override the output child_frame_id
* Fixing Euler body-to-world transformations
* Whitespace
* fixing no datum service in melodic
* Contributors: Alexis schad, Matthew Jones, Tom Moore, thallerod
2.6.2 (2018-10-25)
------------------
* Fixing tests
* Contributors: Tom Moore
2.6.1 (2018-10-25)
------------------
* Adding more output for measurement history failures
* Adding filter processing toggle service
* Waiting for valid ROS time before starting navsat_transform_node
* Contributors: Tom Moore, stevemacenski
2.6.0 (2018-07-27)
------------------
* Moving to C++14, adding error flags, and fixing all warnings
* Contributors: Tom Moore
2.5.2 (2018-04-11)
------------------
* Add published accel topic to documentation
* adding log statements for nans in the invertable matrix
* Fixing issue with potential seg fault
* Contributors: Oleg Kalachev, Tom Moore, stevemacenski
2.5.1 (2018-01-03)
------------------
* Fixing CMakeLists
* Contributors: Tom Moore
2.5.0 (2017-12-15)
------------------
* Fixing datum precision
* Fixing timing variable
* Fixing state history reversion
* Fixing critical bug with dynamic process noise covariance
* Fix typo in reading Mahalanobis thresholds.
* Zero out rotation in GPS to base_link transform
* Update xmlrpcpp includes for Indigo support
* Removing lastUpdateTime
* Fixing timestamps in map->odom transform
* Simplify enabledAtStartup logic
* Add std_srvs dependency
* Add enabling service
* Ensure all raw sensor input orientations are normalized even if messages are not
* Install params directory.
* Add robot localization estimator
* Adding nodelet support
* Contributors: Jacob Perron, Jacob Seibert, Jiri Hubacek, Mike Purvis, Miquel Massot, Pavlo Kolomiiets, Rein Appeldoorn, Rokus Ottervanger, Simon Gene Gottlieb, Tom Moore, stevemacenski
2.4.0 (2017-06-12)
------------------
* Updated documentation
* Added reset_on_time_jump option
* Added feature to optionally publish utm frame as parent in navsat_transform_node
* Moved global callback queue reset
* Added initial_state parameter and documentation
* Fixed ac/deceleration gains default logic
* Added gravity parameter
* Added delay and throttle if tf lookup fails
* Fixed UKF IMUTwistBasicIO test
* Added transform_timeout parameter
* Set gps_odom timestamp before tf2 lookuptransform
* Removed non-portable sincos calls
* Simplified logic to account for correlated error
* Added dynamic process noise covariance calculation
* Fixed catkin_package Eigen warning
* Added optional publication of acceleration state
* Contributors: Brian Gerkey, Enrique Fernandez, Jochen Sprickerhof, Rein Appeldoorn, Simon Gene Gottlieb, Tom Moore
2.3.1 (2016-10-27)
------------------
* Adding gitignore
* Adding remaining wiki pages
* Adding config and prep pages
* Adding navsat_transform_node documentation
* use_odometry_yaw fix for n_t_n
* Fixing issue with manual pose reset when history is not empty
* Getting inverse transform when looking up robot's pose.
* Sphinx documentation
* Removing forward slashes from navsat_transform input topics for template launch file
* Adding example launch and parameter files for a two-level EKF setup with navsat_transform_node
* Adding yaml file for navsat_transform_node, and moving parameter documentation to it.
* Updating EKF and UKF parameter templates with usage comments
* Contributors: Tom Moore, asimay
2.3.0 (2016-07-28)
------------------
* Fixed issues with datum usage and frame_ids
* Fixed comment for wait_for_datum
* Fixing issue with non-zero navsat sensor orientation offsets
* Fixing issue with base_link->gps transform wrecking the 'true' UTM position computation
* Using correct covariance for filtered GPS
* Fixed unitialized odometry covariance bug
* Added filter history and measurement queue behavior
* Changing output timestamp to more accurately use the time stamp of the most recently-processed measurement
* Added TcpNoDelay()
* Added parameter to make transform publishing optional
* Fixed differential handling for pose data so that it doesn't care about the message's frame_id
* Updated UKF config and launch
* Added a test case for the timestamp diagnostics
* Added reporting of bad timestamps via diagnostics
* Updated tests to match new method signatures
* Added control term
* Added smoothing capability for delayed measurements
* Making variables in navsat_transform conform to ROS coding standards
* Contributors: Adel Fakih, Ivor Wanders, Marc Essinger, Tobias Tueylue, Tom Moore
2.2.3 (2016-04-24)
------------------
* Cleaning up callback data structure and callbacks and updating doxygen comments in headers
* Removing MessageFilters
* Removing deprecated parameters
* Adding the ability to handle GPS offsets from the vehicle's origin
* Cleaning up navsat_transform.h
* Making variables in navsat_transform conform to ROS coding standards
2.2.2 (2016-02-04)
------------------
* Updating trig functions to use sincos for efficiency
* Updating licensing information and adding Eigen MPL-only flag
* Added state to imu frame transformation
* Using state orientation if imu orientation is missing
* Manually adding second spin for odometry and IMU data that is passed to message filters
* Reducing delay between measurement reception and filter output
* Zero altitute in intital transform too, when zero altitude param is set
* Fixing regression with conversion back to GPS coordinates
* Switched cropping of orientation data in inovationSubset with mahalanobis check to prevent excluding measurements with orientations bigger/smaller than ± PI
* Fix Jacobian for EKF.
* Removing warning about orientation variables when only their velocities are measured
* Checking for -1 in IMU covariances and ignoring relevant message data
* roslint and catkin_lint applied
* Adding base_link to datum specification, and fixing bug with order of measurement handling when a datum is specified. Also added check to make sure IMU data is transformable before using it.
* Contributors: Adnan Ademovic, Jit Ray Chowdhury, Philipp Tscholl, Tom Moore, ayrton04, kphil
2.2.1 (2015-05-27)
------------------
* Fixed handling of IMU data w.r.t. differential mode and relative mode
2.2.0 (2015-05-22)
------------------
* Added tf2-friendly tf_prefix appending
* Corrected for IMU orientation in navsat_transform
* Fixed issue with out-of-order measurements and pose resets
* Nodes now assume ENU standard for yaw data
* Removed gps_common dependency
* Adding option to navsat_transform_node that enables the use of the heading from the odometry message instead of an IMU.
* Changed frame_id used in setPoseCallback to be the world_frame
* Optimized Eigen arithmetic for signficiant performance boost
* Migrated to tf2
* Code refactoring and reorganization
* Removed roll and pitch from navsat_transform calculations
* Fixed transform for IMU data to better support mounting IMUs in non-standard orientations
* Added feature to navsat_transform_node whereby filtered odometry data can be coverted back into navsat data
* Added a parameter to allow future dating the world_frame->base_link_frame transform.
* Removed deprecated differential setting handler
* Added relative mode
* Updated and improved tests
* Fixing source frame_id in pose data handling
* Added initial covariance parameter
* Fixed bug in covariance copyinh
* Added parameters for topic queue sizes
* Improved motion model's handling of angular velocities when robot has non-zero roll and pitch
* Changed the way differential measurements are handled
* Added diagnostics
2.1.7 (2015-01-05)
------------------
* Added some checks to eliminate unnecessary callbacks
* Updated launch file templates
* Added measurement outlier rejection
* Added failure callbacks for tf message filters
* Added optional broadcast of world_frame->utm transform for navsat_transform_node
* Bug fixes for differential mode and handling of Z acceleration in 2D mode
2.1.6 (2014-11-06)
------------------
* Added unscented Kalman filter (UKF) localization node
* Fixed map->odom tf calculation
* Acceleration data from IMUs is now used in computing the state estimate
* Added 2D mode
2.1.5 (2014-10-07)
------------------
* Changed initial estimate error covariance to be much smaller
* Fixed some debug output
* Added test suite
* Better compliance with REP-105
* Fixed differential measurement handling
* Implemented message filters
* Added navsat_transform_node
2.1.4 (2014-08-22)
------------------
* Adding utm_transform_node to install targets
2.1.3 (2014-06-22)
------------------
* Some changes to ease GPS integration
* Addition of differential integration of pose data
* Some documentation cleanup
* Added UTM transform node and launch file
* Bug fixes
2.1.2 (2014-04-11)
------------------
* Updated covariance correction formulation to "Joseph form" to improve filter stability.
* Implemented new versioning scheme.
2.1.1 (2014-04-11)
------------------
* Added cmake_modules dependency for Eigen support, and added include to silence boost::signals warning from tf include

216
Localizations/Packages/robot_localization/doc/Makefile Normal file
View File

@@ -0,0 +1,216 @@
# Makefile for Sphinx documentation
#
# You can set these variables from the command line.
SPHINXOPTS =
SPHINXBUILD = sphinx-build
PAPER =
BUILDDIR = .build
# User-friendly check for sphinx-build
ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1)
$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/)
endif
# Internal variables.
PAPEROPT_a4 = -D latex_paper_size=a4
PAPEROPT_letter = -D latex_paper_size=letter
ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
# the i18n builder cannot share the environment and doctrees with the others
I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
.PHONY: help
help:
@echo "Please use \`make <target>' where <target> is one of"
@echo " html to make standalone HTML files"
@echo " dirhtml to make HTML files named index.html in directories"
@echo " singlehtml to make a single large HTML file"
@echo " pickle to make pickle files"
@echo " json to make JSON files"
@echo " htmlhelp to make HTML files and a HTML help project"
@echo " qthelp to make HTML files and a qthelp project"
@echo " applehelp to make an Apple Help Book"
@echo " devhelp to make HTML files and a Devhelp project"
@echo " epub to make an epub"
@echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter"
@echo " latexpdf to make LaTeX files and run them through pdflatex"
@echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx"
@echo " text to make text files"
@echo " man to make manual pages"
@echo " texinfo to make Texinfo files"
@echo " info to make Texinfo files and run them through makeinfo"
@echo " gettext to make PO message catalogs"
@echo " changes to make an overview of all changed/added/deprecated items"
@echo " xml to make Docutils-native XML files"
@echo " pseudoxml to make pseudoxml-XML files for display purposes"
@echo " linkcheck to check all external links for integrity"
@echo " doctest to run all doctests embedded in the documentation (if enabled)"
@echo " coverage to run coverage check of the documentation (if enabled)"
.PHONY: clean
clean:
rm -rf $(BUILDDIR)/*
.PHONY: html
html:
$(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html
@echo
@echo "Build finished. The HTML pages are in $(BUILDDIR)/html."
.PHONY: dirhtml
dirhtml:
$(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml
@echo
@echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml."
.PHONY: singlehtml
singlehtml:
$(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml
@echo
@echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml."
.PHONY: pickle
pickle:
$(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle
@echo
@echo "Build finished; now you can process the pickle files."
.PHONY: json
json:
$(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json
@echo
@echo "Build finished; now you can process the JSON files."
.PHONY: htmlhelp
htmlhelp:
$(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp
@echo
@echo "Build finished; now you can run HTML Help Workshop with the" \
".hhp project file in $(BUILDDIR)/htmlhelp."
.PHONY: qthelp
qthelp:
$(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp
@echo
@echo "Build finished; now you can run "qcollectiongenerator" with the" \
".qhcp project file in $(BUILDDIR)/qthelp, like this:"
@echo "# qcollectiongenerator $(BUILDDIR)/qthelp/robot_localization.qhcp"
@echo "To view the help file:"
@echo "# assistant -collectionFile $(BUILDDIR)/qthelp/robot_localization.qhc"
.PHONY: applehelp
applehelp:
$(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp
@echo
@echo "Build finished. The help book is in $(BUILDDIR)/applehelp."
@echo "N.B. You won't be able to view it unless you put it in" \
"~/Library/Documentation/Help or install it in your application" \
"bundle."
.PHONY: devhelp
devhelp:
$(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp
@echo
@echo "Build finished."
@echo "To view the help file:"
@echo "# mkdir -p $$HOME/.local/share/devhelp/robot_localization"
@echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/robot_localization"
@echo "# devhelp"
.PHONY: epub
epub:
$(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub
@echo
@echo "Build finished. The epub file is in $(BUILDDIR)/epub."
.PHONY: latex
latex:
$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
@echo
@echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex."
@echo "Run \`make' in that directory to run these through (pdf)latex" \
"(use \`make latexpdf' here to do that automatically)."
.PHONY: latexpdf
latexpdf:
$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
@echo "Running LaTeX files through pdflatex..."
$(MAKE) -C $(BUILDDIR)/latex all-pdf
@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
.PHONY: latexpdfja
latexpdfja:
$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
@echo "Running LaTeX files through platex and dvipdfmx..."
$(MAKE) -C $(BUILDDIR)/latex all-pdf-ja
@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
.PHONY: text
text:
$(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text
@echo
@echo "Build finished. The text files are in $(BUILDDIR)/text."
.PHONY: man
man:
$(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man
@echo
@echo "Build finished. The manual pages are in $(BUILDDIR)/man."
.PHONY: texinfo
texinfo:
$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
@echo
@echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo."
@echo "Run \`make' in that directory to run these through makeinfo" \
"(use \`make info' here to do that automatically)."
.PHONY: info
info:
$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
@echo "Running Texinfo files through makeinfo..."
make -C $(BUILDDIR)/texinfo info
@echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo."
.PHONY: gettext
gettext:
$(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale
@echo
@echo "Build finished. The message catalogs are in $(BUILDDIR)/locale."
.PHONY: changes
changes:
$(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes
@echo
@echo "The overview file is in $(BUILDDIR)/changes."
.PHONY: linkcheck
linkcheck:
$(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck
@echo
@echo "Link check complete; look for any errors in the above output " \
"or in $(BUILDDIR)/linkcheck/output.txt."
.PHONY: doctest
doctest:
$(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest
@echo "Testing of doctests in the sources finished, look at the " \
"results in $(BUILDDIR)/doctest/output.txt."
.PHONY: coverage
coverage:
$(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage
@echo "Testing of coverage in the sources finished, look at the " \
"results in $(BUILDDIR)/coverage/python.txt."
.PHONY: xml
xml:
$(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml
@echo
@echo "Build finished. The XML files are in $(BUILDDIR)/xml."
.PHONY: pseudoxml
pseudoxml:
$(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml
@echo
@echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml."

197
Localizations/Packages/robot_localization/doc/conf.py Normal file
View File

@@ -0,0 +1,197 @@
import sys
import os
import catkin_pkg.package
catkin_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
catkin_package = catkin_pkg.package.parse_package(os.path.join(catkin_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME))
extensions = [
'sphinx.ext.autodoc',
'sphinx.ext.doctest',
'sphinx.ext.intersphinx',
'sphinx.ext.todo',
'sphinx.ext.coverage',
'sphinx.ext.mathjax',
]
templates_path = ['.templates']
source_suffix = '.rst'
master_doc = 'index'
project = u'robot_localization'
copyright = u'2016, Tom Moore'
author = u'Tom Moore'
version = catkin_package.version
release = catkin_package.version
language = None
exclude_patterns = ['.build']
pygments_style = 'sphinx'
todo_include_todos = True
html_theme = 'nature'
html_theme_options = {
"sidebarwidth": "350"
}
# The name of an image file (relative to this directory) to place at the top
# of the sidebar.
html_logo = 'images/rl_small.png'
# The name of an image file (relative to this directory) to use as a favicon of
# the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
# pixels large.
#html_favicon = None
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
#html_static_path = ['.static']
# Add any extra paths that contain custom files (such as robots.txt or
# .htaccess) here, relative to this directory. These files are copied
# directly to the root of the documentation.
#html_extra_path = []
# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
# using the given strftime format.
#html_last_updated_fmt = '%b %d, %Y'
# If true, SmartyPants will be used to convert quotes and dashes to
# typographically correct entities.
#html_use_smartypants = True
# Custom sidebar templates, maps document names to template names.
html_sidebars = { '**': ['full_globaltoc.html', 'sourcelink.html', 'searchbox.html'], }
# Additional templates that should be rendered to pages, maps page names to
# template names.
#html_additional_pages = {}
# If false, no module index is generated.
#html_domain_indices = True
# If false, no index is generated.
#html_use_index = True
# If true, the index is split into individual pages for each letter.
#html_split_index = False
# If true, links to the reST sources are added to the pages.
#html_show_sourcelink = True
# If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
#html_show_sphinx = True
# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
#html_show_copyright = True
# If true, an OpenSearch description file will be output, and all pages will
# contain a <link> tag referring to it. The value of this option must be the
# base URL from which the finished HTML is served.
#html_use_opensearch = ''
# This is the file name suffix for HTML files (e.g. ".xhtml").
#html_file_suffix = None
# Language to be used for generating the HTML full-text search index.
# Sphinx supports the following languages:
# 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja'
# 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr'
#html_search_language = 'en'
# A dictionary with options for the search language support, empty by default.
# Now only 'ja' uses this config value
#html_search_options = {'type': 'default'}
# The name of a javascript file (relative to the configuration directory) that
# implements a search results scorer. If empty, the default will be used.
#html_search_scorer = 'scorer.js'
# Output file base name for HTML help builder.
htmlhelp_basename = 'robot_localizationdoc'
# -- Options for LaTeX output ---------------------------------------------
latex_elements = {
# The paper size ('letterpaper' or 'a4paper').
#'papersize': 'letterpaper',
# The font size ('10pt', '11pt' or '12pt').
#'pointsize': '10pt',
# Additional stuff for the LaTeX preamble.
#'preamble': '',
# Latex figure (float) alignment
#'figure_align': 'htbp',
}
# Grouping the document tree into LaTeX files. List of tuples
# (source start file, target name, title,
# author, documentclass [howto, manual, or own class]).
latex_documents = [
(master_doc, 'robot_localization.tex', u'robot\\_localization Documentation',
u'Tom Moore', 'manual'),
]
# The name of an image file (relative to this directory) to place at the top of
# the title page.
#latex_logo = None
# For "manual" documents, if this is true, then toplevel headings are parts,
# not chapters.
#latex_use_parts = False
# If true, show page references after internal links.
#latex_show_pagerefs = False
# If true, show URL addresses after external links.
#latex_show_urls = False
# Documents to append as an appendix to all manuals.
#latex_appendices = []
# If false, no module index is generated.
#latex_domain_indices = True
# -- Options for manual page output ---------------------------------------
# One entry per manual page. List of tuples
# (source start file, name, description, authors, manual section).
man_pages = [
(master_doc, 'robot_localization', u'robot_localization Documentation',
[author], 1)
]
# If true, show URL addresses after external links.
#man_show_urls = False
# -- Options for Texinfo output -------------------------------------------
# Grouping the document tree into Texinfo files. List of tuples
# (source start file, target name, title, author,
# dir menu entry, description, category)
texinfo_documents = [
(master_doc, 'robot_localization', u'robot_localization Documentation',
author, 'robot_localization', 'One line description of project.',
'Miscellaneous'),
]
# Documents to append as an appendix to all manuals.
#texinfo_appendices = []
# If false, no module index is generated.
#texinfo_domain_indices = True
# How to display URL addresses: 'footnote', 'no', or 'inline'.
#texinfo_show_urls = 'footnote'
# If true, do not generate a @detailmenu in the "Top" node's menu.
#texinfo_no_detailmenu = False
# Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {'https://docs.python.org/': None}

117
Localizations/Packages/robot_localization/doc/configuring_robot_localization.rst Normal file
View File

@@ -0,0 +1,117 @@
.. _configuring_robot_localization:
Configuring robot_localization
##############################
When incorporating sensor data into the position estimate of any of ``robot_localization``'s state estimation nodes, it is important to extract as much information as possible. This tutorial details the best practices for sensor integration.
For additional information, users are encouraged to watch this `presentation <https://vimeo.com/142624091>`_ from ROSCon 2015.
Sensor Configuration
********************
The configuration vector format is the same for all sensors, even if the message type in question doesn't contain some of the variables in the configuration vector (e.g., a <<MsgLink(geometry_msgs/TwistWithCovarianceStamped)>> lacks any pose data, but the configuration vector still has values for pose variables). Unused variables are simply ignored.
Note that the configuration vector is given in the ``frame_id`` of the input message. For example, consider a velocity sensor that produces a `geometry_msgs/TwistWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html>`_ message with a ``frame_id`` of *velocity_sensor_frame*. In this example, we'll assume that a transform exists from *velocity_sensor_frame* to your robot's ``base_link_frame`` (e.g., *base_link*), and that the transform would convert :math:`X` velocity in the *velocity_sensor_frame* to :math:`Z` velocity in the ``base_link_frame``. To include the :math:`X` velocity data from the sensor into the filter, the configuration vector should set the :math:`X` velocity value to *true*, and **not** the :math:`\dot{Z}` velocity value:
.. code-block:: xml
<rosparam param="twist0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]</rosparam>
.. note:: The order of the boolean values are :math:`(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})`.
Operating in 2D?
****************
The first decision to make when configuring your sensors is whether or not your robot is operating in a planar environment, and you're comfortable with ignoring subtle effects of variations in the ground plane as might be reported from an IMU. If so, please set the ``two_d_mode`` parameter to *true*. This effectively zeros out the 3D pose variables in every measurement and forces them to be fused in the state estimate.
Fusing Unmeasured Variables
***************************
Let's start with an example. Let's say you have a wheeled, nonholonomic robot that works in a planar environment. Your robot has some wheel encoders that are used to estimate instantaneous X velocity as well as absolute pose information. This information is reported in an `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message. Additionally, your robot has an IMU that measures rotational velocity, vehicle attitude, and linear acceleration. Its data is reported in a `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message. As we are operating in a planar environment, we set the ``two_d_mode`` parameter to *true*. This will automatically zero out all 3D variables, such as :math:`Z`, :math:`roll`, :math:`pitch`, their respective velocities, and :math:`Z` acceleration. We start with this configuration:
.. code-block:: xml
<rosparam param="odom0_config">[true, true, false,
false, false, true,
true, false, false,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]</rosparam>
As a first pass, this makes sense, as a planar robot only needs to concern itself with :math:`X`, :math:`Y`, :math:`\dot{X}`, :math:`\dot{Y}`, :math:`\ddot{X}`, :math:`\ddot{Y}`, :math:`yaw`, and :math:`\dot{yaw}`. However, there are a few things to note here.
1. For ``odom0``, we are including :math:`X` and :math:`Y` (reported in a world coordinate frame), :math:`yaw`, :math:`\dot{X}` (reported in the body frame), and :math:`\dot{yaw}`. However, unless your robot is internally using an IMU, it is most likely simply using wheel encoder data to generate the values in its measurements. Therefore, its velocity, heading, and position data are all generated from the same source. In that instance, we don't want to use all the values, as you're feeding duplicate information into the filter. Instead, it's best to just use the velocities:
.. code-block:: xml
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]</rosparam>
2. Next, we note that we are not fusing :math:`\dot{Y}`. At first glance, this is the right choice, as our robot cannot move instantaneously sideways. However, if the `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message reports a :math:`0` value for :math:`\dot{Y}` (and the :math:`\dot{Y}` covariance is NOT inflated to a large value), it's best to feed that value to the filter. As a :math:`0` measurement in this case indicates that the robot cannot ever move in that direction, it serves as a perfectly valid measurement:
.. code-block:: xml
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]</rosparam>
You may wonder why did we not fuse :math:`\dot{Z}` velocity for the same reason. The answer is that we did when we set ``two_d_mode`` to *false*. If we hadn't, we could, in fact, fuse the :math:`0` measurement for :math:`\dot{Z}` velocity into the filter.
3. Last, we come to the IMU. You may notice that we have set the :math:`\ddot{Y}` to *false*. This is due to the fact that many systems, including the hypothetical one we are discussing here, will not undergo instantaneous :math:`Y` acceleration. However, the IMU will likely report non-zero, noisy values for Y acceleration, which can cause your estimate to drift rapidly.
The *differential* Parameters
*****************************
The state estimation nodes in ''robot_localization'' allow users to fuse as many sensors as they like. This allows users to measure certain state vector variables - in particular, pose variables - using more than one source. For example, your robot may obtain absolute orientation information from multiple IMUs, or it may have multiple data sources providing an estimate its absolute position. In this case, users have two options:
1. Fuse all the absolute position/orientation data as-is, e.g.,
.. code-block:: xml
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu1_config">[false, false, false,
true, true, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
In this case, users should be **very** careful and ensure that the covariances on each measured orientation variable are set correctly. If each IMU advertises a yaw variance of, for example, :math:`0.1`, yet the delta between the IMUs' yaw measurements is :math:`> 0.1`, then the output of the filter will oscillate back and forth between the values provided by each sensor. Users should make sure that the noise distributions around each measurement overlap.
2. Alternatively, users can make use of the ``_differential`` parameter. By setting this to *true* for a given sensor, all pose (position and orientation) data is converted to a velocity by calculating the change in the measurement value between two consecutive time steps. The data is then fused as a velocity. Again, though, users should take care: when measurements are fused absolutely (especially IMUs), if the measurement has a static or non-increasing variance for a given variable, then the variance in the estimate's covariance matrix will be bounded. If that information is converted to a velocity, then at each time step, the estimate will gain some small amount of error, and the variance for the variable in question will grow without bound. For position :math:`(X, Y, Z)` information, this isn't an issue, but for orientation data, it is a problem. For example, it is acceptable for a robot to move around its environment and accumulate :math:`1.5` meters of error in the :math:`X` direction after some time. If that same robot moves around and accumulates :math:`1.5` radians of error in yaw, then when the robot next drives forward, its position error will explode.
The general rule of thumb for the ``_differential`` parameter is that if a give robot has only one source of orientation data, then the differential parameter should be set to *false*. If there are :math:`N` sources, users can set the ``_differential`` parameter to *true* for :math:`N-1` of them, or simply ensure that the covariance values are large enough to eliminate oscillations.

BIN
Localizations/Packages/robot_localization/doc/images/figure2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 82 KiB

BIN
Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 50 KiB

49
Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.tex Normal file
View File

@@ -0,0 +1,49 @@
\documentclass{standalone}
\usepackage{tikz}
\usetikzlibrary{positioning}
\begin{document}
\sffamily
\begin{tikzpicture}[>=stealth,
node distance = 3cm,
box/.style={shape=rectangle,draw,rounded corners},]
% Nodes
\node (wheel) [box] {Wheel Odometry};
\node (imu) [box, right =of wheel]{IMU};
\node (ekfLocal) [box, below =of wheel]{EKF Local};
\node (ekfGlobal) [box, below =of imu]{EKF Global};
\node (navsat) [box, right =7cm of ekfGlobal]{navsat\_transform};
\node (gps) [box, above =of navsat]{GPS};
% Coordinates for edges pointing to empty space
\node (gpsF) [right =of navsat]{};
\node (tfLocal) [below =of ekfLocal]{};
\node (odomLocal) [left =of ekfLocal]{};
\node (tfGlobal) [below =of ekfGlobal]{};
% Edges
\draw[->] (wheel.270) -- (ekfLocal.90);
\draw[->] (wheel.315) -- (ekfGlobal.135)
node [fill=white, pos=0.2, align=center] {\ttfamily"/wheel/odometry"\\nav\_msgs/Odometry};
\draw[->] (imu.225) -- (ekfLocal.45);
\draw[->] (imu.315) -- (navsat.135);
\draw[->] (imu.270) -- (ekfGlobal.90)
node [fill=white, pos=0.2, align=center] {\ttfamily"/imu/data"\\sensor\_msgs/Imu};
\draw[->] (gps.270) -- (navsat.90)
node [fill=white, pos=0.2, align=center] {\ttfamily"/gps/fix"\\sensor\_msgs/NavSatFix};
\draw[->, transform canvas={yshift=1mm}] (ekfGlobal) -- (navsat)
node [fill=white, above=1mm, pos=0.5, align=center] {\ttfamily"/odometry/filtered/global"\\nav\_msgs/Odometry};
\draw[->, transform canvas={yshift=-1mm}] (navsat) -- (ekfGlobal)
node [fill=white, below=1mm, pos=0.5, align=center] {\ttfamily"/odometry/gps"\\nav\_msgs/Odometry};
% Outputs not cycled back into the graph
\draw[->, dashed] (navsat) -- (gpsF)
node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/gps/filtered"\\sensor\_msgs/NavSatFix};
\draw[->, dashed] (ekfLocal) -- (tfLocal)
node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" odom -> base\\tf2\_msgs/TFMessage};
\draw[->, dashed] (ekfLocal) -- (odomLocal)
node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/odometry/filtered/local"\\nav\_msgs/Odometry};
\draw[->, dashed] (ekfGlobal) -- (tfGlobal)
node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" map -> odom\\tf2\_msgs/TFMessage};
\end{tikzpicture}
\end{document}

BIN
Localizations/Packages/robot_localization/doc/images/rl_small.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 58 KiB

55
Localizations/Packages/robot_localization/doc/index.rst Normal file
View File

@@ -0,0 +1,55 @@
.. _index:
robot_localization wiki
***********************
``robot_localization`` is a collection of state estimation nodes, each of which is an implementation of a nonlinear state estimator for robots moving in 3D space. It contains two state estimation nodes, ``ekf_localization_node`` and ``ukf_localization_node``. In addition, ``robot_localization`` provides ``navsat_transform_node``, which aids in the integration of GPS data.
.. toctree::
:hidden:
state_estimation_nodes
navsat_transform_node
preparing_sensor_data
configuring_robot_localization
migrating_from_robot_pose_ekf
integrating_gps
CHANGELOG
user_contributed_tutorials
Features
========
All the state estimation nodes in ``robot_localization`` share common features, namely:
* Fusion of an arbitrary number of sensors. The nodes do not restrict the number of input sources. If, for example, your robot has multiple IMUs or multiple sources of odometry information, the state estimation nodes within ``robot_localization`` can support all of them.
* Support for multiple ROS message types. All state estimation nodes in ``robot_localization`` can take in `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_, `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_, `geometry_msgs/PoseWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html>`_, or `geometry_msgs/TwistWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html>`_ messages.
* Per-sensor input customization. If a given sensor message contains data that you don't want to include in your state estimate, the state estimation nodes in ``robot_localization`` allow you to exclude that data on a per-sensor basis.
* Continuous estimation. Each state estimation node in ``robot_localization`` begins estimating the vehicle's state as soon as it receives a single measurement. If there is a holiday in the sensor data (i.e., a long period in which no data is received), the filter will continue to estimate the robot's state via an internal motion model.
All state estimation nodes track the 15-dimensional state of the vehicle: :math:`(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})`.
Other Resources
===============
If you're new to ``robot_localization``, check out the `2015 ROSCon talk <https://vimeo.com/142624091>`_ for some pointers on getting started.
Further details can be found in :download:`this paper <robot_localization_ias13_revised.pdf>`:
.. code-block:: none
@inproceedings{MooreStouchKeneralizedEkf2014,
author = {T. Moore and D. Stouch},
title = {A Generalized Extended Kalman Filter Implementation for the Robot Operating System},
year = {2014},
month = {July},
booktitle = {Proceedings of the 13th International Conference on Intelligent Autonomous Systems (IAS-13)},
publisher = {Springer}
}
Indices and tables
==================
* :ref:`genindex`
* :ref:`search`

165
Localizations/Packages/robot_localization/doc/integrating_gps.rst Normal file
View File

@@ -0,0 +1,165 @@
Integrating GPS Data
####################
Integration of GPS data is a common request from users. ``robot_localization`` contains a node, ``navsat_transform_node``, that transforms GPS data into a frame that is consistent with your robot's starting pose (position and orientation) in its world frame. This greatly simplifies fusion of GPS data. This tutorial explains how to use ``navsat_transform_node``, and delves into some of the math behind it.
For additional information, users are encouraged to watch this `presentation <https://vimeo.com/142624091>`_ from ROSCon 2015.
Notes on Fusing GPS Data
************************
Before beginning this tutorial, users should be sure to familiarize themselves with `REP-105 <http://www.ros.org/reps/rep-0105.html>`_. It is important for users to realize that using a position estimate that includes GPS data will likely be unfit for use by navigation modules, owing to the fact that GPS data is subject to discrete discontinuities ("jumps"). If you want to fuse data from a GPS into your position estimate, one potential solution is to do the following:
1. Run one instance of a ``robot_localization`` state estimation node that fuses only continuous data, such as odometry and IMU data. Set the ``world_frame`` parameter for this instance to the same value as the ``odom_frame`` parameter. Execute local path plans and motions in this frame.
2. Run another instance of a ``robot_localization`` state estimation node that fuses all sources of data, including the GPS. Set the ``world_frame`` parameter for this instance to the same value as the ``map_frame`` parameter.
This is just a suggestion, however, and users are free to fuse the GPS data into a single instance of a ``robot_localization`` state estimation node.
Using navsat_transform_node
***************************
The diagram below illustrates an example setup:
.. image:: images/navsat_transform_workflow.png
:width: 800px
:align: center
:alt: navsat_transform workflow
Required Inputs
===============
``navsat_transform_node`` requires three sources of information: the robot's current pose estimate in its world frame, an earth-referenced heading, and a geographic coordinate expressed as a latitude/longitude pair (with optional altitude).
These data can be obtained in three different ways:
1. (Default behavior) The data can come entirely from the robot's sensors and pose estimation software. To enable this mode, make sure the ``wait_for_datum`` parameter is set to *false* (its default value). The required messages are:
* A `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message with raw GPS coordinates in it.
* A `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with an absolute (earth-referenced) heading.
* A `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message that contains the robot's current position estimate in the frame specified by its start location (typically the output of a ``robot_localization`` state estimation node).
2. The datum (global frame origin) can be specified via the ``datum`` parameter.
.. note:: In order to use this mode, the ``wait_for_datum`` parameter must be set to *true*.
The ``datum`` parameter takes this form:
.. code-block:: xml
<rosparam param="datum">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>
The parameter order is ``latitude`` in decimal degrees, ``longitude`` in decimal degrees, ``heading`` in radians) the ``frame_id`` of your robot's world frame (i.e., the value of the ``world_frame`` parameter in a ``robot_localization`` state estimation node), and the ``frame_id`` of your robot's body frame (i.e., the value of the ``base_link_frame`` parameter in a ``robot_localization`` state estimation node). When this mode is used, the robot assumes that your robot's world frame origin is at the specified latitude and longitude and with a heading of :math:`0` (east).
3. The datum can be set manually via the ``set_datum`` service and using the `robot_localization/SetDatum <http://docs.ros.org/api/robot_localization/html/srv/SetDatum.html>`_ service message.
GPS Data
^^^^^^^^
Please note that all development of ``navsat_transform_node`` was done using a Garmin 18x GPS unit, so there may be intricacies of the data generated by other units that need to be handled.
The excellent `nmea_navsat_driver <http://wiki.ros.org/nmea_navsat_driver>`_ package provides the required `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message. Here is the ``nmea_navsat_driver`` launch file we'll use for this tutorial:
.. code-block:: xml
<node pkg="nmea_navsat_driver" type="nmea_serial_driver" name="navsat" respawn="true">
<param name="port" value="/dev/ttyUSB0"/>
<param name="baud" value="19200"/>
</node>
This information is only relevant if the user is not manually specifying the origin via the ``datum`` parameter or the ``set_datum`` service.
IMU Data
^^^^^^^^
.. note:: Since version 2.2.1, ``navsat_transform_node`` has moved to a standard wherein all heading data is assumed to start with its zero point facing east. If your IMU does not conform to this standard and instead reports zero when facing north, you can still use the ``yaw_offset`` parameter to correct this. In this case, the value for ``yaw_offset`` would be :math:`\pi / 2` (approximately :math:`1.5707963`).
Users should make sure their IMUs conform to `REP-105 <http://www.ros.org/reps/rep-0105.html>`_. In particular, check that the signs of your orientation angles increase in the right direction. In addition, users should look up the `magnetic declination <http://www.ngdc.noaa.gov/geomag-web/#declination>`_ for their robot's operating area, convert it to radians, and then use that value for the ``magnetic_declination_radians`` parameter.
This information is only relevant if the user is not manually specifying the origin via the ``datum`` parameter or the ``set_datum`` service.
Odometry Data
^^^^^^^^^^^^^
This should just be the output of whichever ``robot_localization`` state estimation node instance you are using to fuse GPS data.
Configuring navsat_transform_node
=================================
Below is the ``navsat_transform_node`` launch file we'll use for this tutorial:
.. code-block:: xml
<launch>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<remap from="/imu/data" to="/your/imu/topic" />
<remap from="/gps/fix" to="/your/gps/fix/topic" />
<remap from="/odometry/filtered" to="/your/robot_localization/output/topic" />
</node>
</launch>
These parameters are discussed on the :ref:`main page <index>`.
Configuring robot_localization
==============================
Integration with ``robot_localization`` is straightforward at this point. Simply add this block to your state estimation node launch file:
.. code-block:: xml
<param name="odomN" value="/your_state_estimation_node_topic">
<rosparam param="odomN_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odomN_differential" value="false"/>
Make sure to change ``odomN`` to whatever your odometry input values is (e.g., *odom1*, *odom2*, etc.). Also, if you wish to include altitude data, set ``odomN_config``'s third value to ``true``.
.. note:: If you are operating in 2D don't have any sensor measuring Z or Z velocity, you can either:
* Set ``navsat_transform_node's`` ``zero_altitude`` parameter to *true*, and then set ``odomN_config``'s third value to *true*
* Set ``two_d_mode`` to *true* in your ``robot_localization`` state estimation node
You should have no need to modify the ``_differential`` setting within the state estimation node. The GPS is an absolute position sensor, and enabling differential integration defeats the purpose of using it.
Details
=======
We'll start with a picture. Consider a robot that starts at some latitude and longitude and with some heading. We assume in this tutorial that the heading comes from an IMU that reads 0 when facing east, and increases according to the ROS spec (i.e., counter-clockwise). The remainder of this tutorial will refer to Figure 2:
.. image:: images/figure2.png
:width: 800px
:align: center
:alt: Figure 2
`REP-105 <http://www.ros.org/reps/rep-0105.html>`_ suggests four coordinate frames: *base_link*, *odom*, *map*, and *earth*. *base_link* is the coordinate frame that is rigidly attached to the vehicle. The *odom* and *map* frames are world-fixed frames and generally have their origins at the vehicle's start position and orientation. The *earth* frame is used as a common reference frame for multiple map frames, and is not yet supported by ``navsat_transform_node``. Note that in Figure 2, the robot has just started (``t = 0``), and so its *base_link*, *odom*, and *map* frames are aligned. We can also define a coordinate frame for the UTM grid, which we will call *utm*. For the purposes of this tutorial, we will refer to the UTM grid coordinate frame as *utm*. Therefore, what we want to do is create a *utm*->*map* transform.
Referring to Figure 2, these ideas are (hopefully) made clear. The UTM origin is the :math:`(0_{UTM}, 0_{UTM})` point of the UTM zone that is associated with the robot's GPS location. The robot begins somewhere within the UTM zone at location :math:`(x_{UTM}, y_{UTM})`. The robot's initial orientation is some angle :math:`\theta` above the UTM grid's :math:`X`-axis. Our transform will therefore require that we know :math:`x_{UTM}, y_{UTM}` and :math:`\theta`.
We now need to convert our latitude and longitude to UTM coordinates. The UTM grid assumes that the :math:`X`-axis faces east, the :math:`Y`-axis faces (true) north, and the :math:`Z`-axis points up out of the ground. This complies with the right-handed coordinate frame as dictated by `REP-105 <http://www.ros.org/reps/rep-0105.html>`_. The REP also states that a yaw angle of :math:`0` means that we are facing straight down the :math:`X`-axis, and that the yaw increases counter-clockwise. ``navsat_transform_node`` assumes your heading data conforms to this standard. However, there are two factors that need to be considered:
1. The IMU driver may not allow the user to apply the magnetic declination correction factor
2. The IMU driver may incorrectly report :math:`0` when facing north, and not when facing east (even though its headings increase and decrease correctly). Fortunately, ``navsat_transform_node`` exposes two parameters to adddress these possible shortcomings in IMU data: ``magnetic_declination_radians`` and ``yaw_offset``. Referring to Figure 2, for an IMU that is currently measuring a yaw value of ``imu_yaw``,
:math:`yaw_{imu} = -\omega - offset_{yaw} + \theta`
:math:`\theta = yaw_{imu} + \omega + offset_{yaw}`
We now have a translation :math:`(x_{UTM}, y_{UTM})` and rotation :math:`\theta`, which we can use to create the required *utm* -> *map* transform. We use the transform to convert all future GPS positions into the robot's local coordinate frame. ``navsat_transform_node`` will also broadcast this transform if the ``broadcast_utm_transform`` parameter is set to *true*.
If you have any questions about this tutorial, please feel free to ask questions on `answers.ros.org <http://answers.ros.org>`_.

38
Localizations/Packages/robot_localization/doc/manifest.yaml Normal file
View File

@@ -0,0 +1,38 @@
actions: []
authors: Tom Moore <ayrton04@gmail.com>
brief: ''
bugtracker: ''
depends:
- catkin
- eigen
- diagnostic_updater
- cmake_modules
- tf2
- nav_msgs
- roscpp
- rostest
- tf2_ros
- message_generation
- message_filters
- tf2_geometry_msgs
- sensor_msgs
- message_runtime
- std_msgs
- roslint
- rosunit
- diagnostic_msgs
- geographic_msgs
- python-catkin-pkg
- geometry_msgs
- rosbag
description: The robot_localization package provides nonlinear state estimation through
sensor fusion of an abritrary number of sensors.
license: BSD
maintainers: Tom Moore <ayrton04@gmail.com>
msgs: []
package_type: package
repo_url: ''
srvs:
- SetPose
- SetDatum
url: http://ros.org/wiki/robot_localization

29
Localizations/Packages/robot_localization/doc/migrating_from_robot_pose_ekf.rst Normal file
View File

@@ -0,0 +1,29 @@
.. _migrating_from_robot_pose_ekf:
Migrating from robot_pose_ekf
#############################
Migration from ``robot_pose_ekf`` is fairly straightforward. This page is meant to highlight relevant differences between the packages to facilitate rapid transitions.
Covariances in Source Messages
==============================
For ``robot_pose_ekf``, a common means of getting the filter to ignore measurements is to give it a massively inflated covariance, often on the order of 10^3. However, the state estimation nodes in ``robot_localization`` allow users to specify *which* variables from the measurement should be fused with the current state. If your sensor reports zero for a given variable and you don't want to fuse that value with your filter, or if the sensor is known to produce poor data for that field, then simply set its ``xxxx_config`` parameter value to false for the variable in question (see the main page for a description of this parameter).
However, users should take care: sometimes platform constraints create implicit :math:`0` measurements of variables. For example, a differential drive robot that cannot move instantaneously in the :math:`Y` direction can safely fuse a :math:`0` measurement for :math:`\dot{Y}` with a small covariance value.
The ''differential'' Parameter
==============================
By default, ``robot_pose_ekf`` will take a pose measurement at time :math:`t`, determine the difference between it and the measurement at time :math:`t-1`, transform that difference into the current frame, and then integrate that measurement. This cleverly aids in cases where two sensors are measuring the same pose variable: as time progresses, the values reported by each sensor will start to diverge. If the covariances on at least one of these measurements do not grow appopriately, the filter will eventually start to oscillate between the measured values. By carrying out differential integration, this situation is avoided and measurements are always consistent with the current state.
This situation can be avoided using three different methods for the ``robot_localization`` state estimation nodes:
1. If fusing two different sources for the same pose data (e.g., two different sensors measuring :math:`Z` position), ensure that those sources accurately report their covariances. If the two sources begin to diverge, then their covariances should refect the growing error that must be occurring in at least one of them.
2. If available, fuse velocity data instead of pose data. If you have two separate data sources measuring the same variable, fuse the most accurate one as pose data and the other as velocity.
3. As an alternative to (2), if velocity data is unavailable for a given pose measurement, enable the ``_differential`` parameter for one of the sensors. This will cause it to be differentiated and fused as a velocity.

75
Localizations/Packages/robot_localization/doc/navsat_transform_node.rst Normal file
View File

@@ -0,0 +1,75 @@
navsat_transform_node
*********************
``navsat_transform_node`` takes as input a `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message (usually the output of ``ekf_localization_node`` or ``ukf_localization_node``), a `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ containing an accurate estimate of your robot's heading, and a `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing GPS data. It produces an odometry message in coordinates that are consistent with your robot's world frame. This value can be directly fused into your state estimate.
.. note:: If you fuse the output of this node with any of the state estimation nodes in ``robot_localization``, you should make sure that the ``odomN_differential`` setting is *false* for that input.
Parameters
==========
~frequency
^^^^^^^^^^
The real-valued frequency, in Hz, at which ``navsat_transform_node`` checks for new `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ messages, and publishes filtered `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ when ``publish_filtered_gps`` is set to *true*.
~delay
^^^^^^
The time, in seconds, to wait before calculating the transform from GPS coordinates to your robot's world frame.
~magnetic_declination_radians
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Enter the magnetic declination for your location. If you don't know it, see `http://www.ngdc.noaa.gov/geomag-web <http://www.ngdc.noaa.gov/geomag-web>`_ (make sure to convert the value to radians). This parameter is needed if your IMU provides its orientation with respect to the magnetic north.
~yaw_offset
^^^^^^^^^^^
Your IMU should read 0 for yaw when facing east. If it doesn't, enter the offset here (desired_value = offset + sensor_raw_value). For example, if your IMU reports 0 when facing north, as most of them do, this parameter would be ``pi/2`` (~1.5707963). This parameter changed in version ``2.2.1``. Previously, ``navsat_transform_node`` assumed that IMUs read 0 when facing north, so yaw_offset was used acordingly.
~zero_altitude
^^^^^^^^^^^^^^
If this is *true*, the `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message produced by this node has its pose Z value set to 0.
~publish_filtered_gps
^^^^^^^^^^^^^^^^^^^^^
If *true*, ``navsat_transform_node`` will also transform your robot's world frame (e.g., *map*) position back to GPS coordinates, and publish a `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message on the ``/gps/filtered`` topic.
~broadcast_utm_transform
^^^^^^^^^^^^^^^^^^^^^^^^
If this is *true*, ``navsat_transform_node`` will broadcast the transform between the UTM grid and the frame of the input odometry data. See Published Transforms below for more information.
~use_odometry_yaw
^^^^^^^^^^^^^^^^^
If *true*, ``navsat_transform_node`` will not get its heading from the IMU data, but from the input odometry message. Users should take care to only set this to true if your odometry message has orientation data specified in an earth-referenced frame, e.g., as produced by a magnetometer. Additionally, if the odometry source is one of the state estimation nodes in ``robot_localization``, the user should have at least one source of absolute orientation data being fed into the node, with the ``_differential`` and ``_relative`` parameters set to *false*.
~wait_for_datum
^^^^^^^^^^^^^^^
If *true*, ``navsat_transform_node`` will wait to get a datum from either:
* The ``datum`` parameter
* The ``set_datum`` service
~broadcast_utm_transform_as_parent_frame
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
If *true*, ``navsat_transform_node`` will publish the utm->world_frame transform instead of the world_frame->utm transform.
Note that for the transform to be published ``broadcast_utm_transform`` also has to be set to *true*.
~transform_timeout
^^^^^^^^^^^^^^^^^^
This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see ``tf2`` implementation) transform.
Subscribed Topics
=================
* ``imu/data`` A `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with orientation data
* ``odometry/filtered`` A `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message of your robot's current position. This is needed in the event that your first GPS reading comes after your robot has attained some non-zero pose.
* ``gps/fix`` A `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing your robot's GPS coordinates
Published Topics
================
* ``odometry/gps`` A `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message containing the GPS coordinates of your robot, transformed into its world coordinate frame. This message can be directly fused into ``robot_localization``'s state estimation nodes.
* ``gps/filtered`` (optional) A `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing your robot's world frame position, transformed into GPS coordinates
Published Transforms
====================
* ``world_frame->utm`` (optional) - If the ``broadcast_utm_transform`` parameter is set to *true*, ``navsat_transform_node`` calculates a transform from the *utm* frame to the ``frame_id`` of the input odometry data. By default, the *utm* frame is published as a child of the odometry frame by using the inverse transform. With use of the ``broadcast_utm_transform_as_parent_frame`` parameter, the *utm* frame will be published as a parent of the odometry frame. This is useful if you have multiple robots within one TF tree.

104
Localizations/Packages/robot_localization/doc/preparing_sensor_data.rst Normal file
View File

@@ -0,0 +1,104 @@
Preparing Your Data for Use with robot_localization
###################################################
Before getting started with the state estimation nodes in ``robot_localization``, it is important that users ensure that their sensor data well-formed. There are various considerations for each class of sensor data, and users are encouraged to read this tutorial in its entirety before attempting to use ``robot_localization``.
For additional information, users are encouraged to watch this `presentation <https://vimeo.com/142624091>`_ from ROSCon 2015.
Adherence to ROS Standards
**************************
The two most important ROS REPs to consider are:
* `REP-103 <http://www.ros.org/reps/rep-0103.html>`_ (Standard Units of Measure and Coordinate Conventions)
* `REP-105 <http://www.ros.org/reps/rep-0105.html>`_ (Coordinate Frame Conventions).
Users who are new to ROS or state estimation are encouraged to read over both REPs, as it will almost certainly aid you in preparing your sensor data. ``robot_localization`` attempts to adhere to these standards as much as possible.
Also, it will likely benefit users to look over the specifications for each of the supported ROS message types:
* `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_
* `geometry_msgs/PoseWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html>`_
* `geometry_msgs/TwistWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html>`_
* `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_
Coordinate Frames and Transforming Sensor Data
**********************************************
`REP-105 <http://www.ros.org/reps/rep-0105.html>`_ specifies four principal coordinate frames: *base_link*, *odom*, *map*, and *earth*. The *base_link* frame is rigidly affixed to the robot. The *map* and *odom* frames are world-fixed frames whose origins are typically aligned with the robot's start position. The *earth* frame is used to provide a common reference frame for multiple *map* frames (e.g., for robots distributed over a large area). The *earth* frame is not relevant to this tutorial.
The state estimation nodes of ``robot_localization`` produce a state estimate whose pose is given in the *map* or *odom* frame and whose velocity is given in the *base_link* frame. All incoming data is transformed into one of these coordinate frames before being fused with the state. The data in each message type is transformed as follows:
* `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ - All pose data (position and orientation) is transformed from the message header's ``frame_id`` into the coordinate frame specified by the ``world_frame`` parameter (typically *map* or *odom*). In the message itself, this specifically refers to everything contained within the ``pose`` property. All twist data (linear and angular velocity) is transformed from the ``child_frame_id`` of the message into the coordinate frame specified by the ``base_link_frame`` parameter (typically *base_link*).
* `geometry_msgs/PoseWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html>`_ - Handled in the same fashion as the pose data in the Odometry message.
* `geometry_msgs/TwistWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/TwistWithCovarianceStamped.html>`_ - Handled in the same fashion as the twist data in the Odometry message.
* `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ - The IMU message is currently subject to some ambiguity, though this is being addressed by the ROS community. Most IMUs natively report orientation data in a world-fixed frame whose :math:`X` and :math:`Z` axes are defined by the vectors pointing to magnetic north and the center of the earth, respectively, with the Y axis facing east (90 degrees offset from the magnetic north vector). This frame is often referred to as NED (North, East, Down). However, `REP-103 <http://www.ros.org/reps/rep-0103.html>`_ specifies an ENU (East, North, Up) coordinate frame for outdoor navigation. As of this writing, ``robot_localization`` assumes an ENU frame for all IMU data, and does not work with NED frame data. This may change in the future, but for now, users should ensure that data is transformed to the ENU frame before using it with any node in ``robot_localization``.
The IMU may also be oriented on the robot in a position other than its "neutral" position. For example, the user may mount the IMU on its side, or rotate it so that it faces a direction other than the front of the robot. This offset is typically specified by a static transform from the ``base_link_frame`` parameter to the IMU message's ``frame_id``. The state estimation nodes in ``robot_localization`` will automatically correct for the orientation of the sensor so that its data aligns with the frame specified by the ``base_link_frame`` parameter.
Handling tf_prefix
******************
With the migration to `tf2 <http://wiki.ros.org/tf2>`_ as of ROS Indigo, ``robot_localization`` still allows for the use of the ``tf_prefix`` parameter, but, in accordance with `tf2 <http://wiki.ros.org/tf2>`_, all ``frame_id`` values will have any leading '/' stripped.
Considerations for Each Sensor Message Type
*******************************************
Odometry
========
Many robot platforms come equipped with wheel encoders that provide instantaneous translational and rotational velocity. Many also internally integrate these velocities to generate a position estimate. If you are responsible for this data, or can edit it, keep the following in mind:
1. **Velocities/Poses:** `robot_localization` can integrate velocities or absolute pose information. In general, the best practice is:
* If the odometry provides both position and linear velocity, fuse the linear velocity.
* If the odometry provides both orientation and angular velocity, fuse the orientation.
.. note:: If you have two sources of orientation data, then you'll want to be careful. If both produce orientations with accurate covariance matrices, it's safe to fuse the orientations. If, however, one or both under-reports its covariance, then you should only fuse the orientation data from the more accurate sensor. For the other sensor, use the angular velocity (if it's provided), or continue to fuse the absolute orientation data, but turn `_differential` mode on for that sensor.
2. **frame_id:** See the section on coordinate frames and transforms above.
3. **Covariance:** Covariance values **matter** to ``robot_localization``. `robot_pose_ekf <http://wiki.ros.org/robot_pose_ekf>`_ attempts to fuse all pose variables in an odometry message. Some robots' drivers have been written to accommodate its requirements. This means that if a given sensor does not produce a certain variable (e.g., a robot that doesn't report :math:`Z` position), then the only way to get ``robot_pose_ekf`` to ignore it is to inflate its variance to a very large value (on the order of :math:`1e3`) so that the variable in question is effectively ignored. This practice is both unnecessary and even detrimental to the performance of ``robot_localization``. The exception is the case where you have a second input source measuring the variable in question, in which case inflated covariances will work.
.. note:: See :ref:`configuring_robot_localization` and :ref:`migrating_from_robot_pose_ekf` for more information.
4. **Signs:** Adherence to `REP-103 <http://www.ros.org/reps/rep-0103.html>`_ means that you need to ensure that the **signs** of your data are correct. For example, if you have a ground robot and turn it counter-clockwise, then its yaw angle should *increase*, and its yaw velocity should be *positive*. If you drive it *forward*, its X-position should *increase* and its X-velocity should be *positive*.
5. **Transforms:** Broadcast of the *odom*->*base_link* transform. When the ``world_frame`` parameter is set to the value of the ``odom_frame`` parameter in the configuration file, ``robot_localization``'s state estimation nodes output both a position estimate in a `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message and a transform from the frame specified by its ``odom_frame`` parameter to its ``base_link_frame`` parameter. However, some robot drivers also broadcast this transform along with their odometry message. If users want ``robot_localization`` to be responsible for this transform, then they need to disable the broadcast of that transform by their robot's driver. This is often exposed as a parameter.
IMU
===
In addition to the following, be sure to read the above section regarding coordinate frames and transforms for IMU data.
1. **Adherence to specifications:** As with odometry, be sure your data adheres to `REP-103 <http://www.ros.org/reps/rep-0103.html>`_ and the `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ specification. Double-check the signs of your data, and make sure the ``frame_id`` values are correct.
2. **Covariance:** Echoing the advice for odometry, make sure your covariances make sense. Do not use large values to get the filter to ignore a given variable. Set the configuration for the variable you'd like to ignore to *false*.
3. **Acceleration:** Be careful with acceleration data. The state estimation nodes in ``robot_localization`` assume that an IMU that is placed in its neutral *right-side-up* position on a flat surface will:
* Measure **+**:math:`9.81` meters per second squared for the :math:`Z` axis.
* If the sensor is rolled **+**:math:`90` degrees (left side up), the acceleration should be **+**:math:`9.81` meters per second squared for the :math:`Y` axis.
* If the sensor is pitched **+**:math:`90` degrees (front side down), it should read **-**:math:`9.81` meters per second squared for the :math:`X` axis.
PoseWithCovarianceStamped
=========================
See the section on odometry.
TwistWithCovarianceStamped
==========================
See the section on odometry.
Common errors
*************
* Input data doesn't adhere to `REP-103 <http://www.ros.org/reps/rep-0103.html>`_. Make sure that all values (especially orientation angles) increase and decrease in the correct directions.
* Incorrect ``frame_id`` values. Velocity data should be reported in the frame given by the ``base_link_frame`` parameter, or a transform should exist between the ``frame_id`` of the velocity data and the ``base_link_frame``.
* Inflated covariances. The preferred method for ignoring variables in measurements is through the ``odomN_config`` parameter.
* Missing covariances. If you have configured a given sensor to fuse a given variable into the state estimation node, then the variance for that value (i.e., the covariance matrix value at position :math:`(i, i)`, where :math:`i` is the index of that variable) should **not** be :math:`0`. If a :math:`0` variance value is encountered for a variable that is being fused, the state estimation nodes will add a small epsilon value (:math:`1e^{-6}`) to that value. A better solution is for users to set covariances appropriately.

BIN
Localizations/Packages/robot_localization/doc/robot_localization_ias13_revised.pdf Normal file
View File

Binary file not shown.

336
Localizations/Packages/robot_localization/doc/state_estimation_nodes.rst Normal file
View File

@@ -0,0 +1,336 @@
State Estimation Nodes
######################
ekf_localization_node
*********************
``ekf_localization_node`` is an implementation of an `extended Kalman filter <http://en.wikipedia.org/wiki/Extended_Kalman_filter>`_. It uses an omnidirectional motion model to project the state forward in time, and corrects that projected estimate using perceived sensor data.
ukf_localization_node
*********************
``ukf_localization_node`` is an implementation of an `unscented Kalman filter <http://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter>`_. It uses a set of carefully selected sigma points to project the state through the same motion model that is used in the EKF, and then uses those projected sigma points to recover the state estimate and covariance. This eliminates the use of Jacobian matrices and makes the filter more stable. However, it is also more computationally taxing than ``ekf_localization_node``.
Parameters
**********
``ekf_localization_node`` and ``ukf_localization_node`` share the vast majority of their parameters, as most of the parameters control how data is treated before being fused with the core filters.
The relatively large number of parameters available to the state estimation nodes make launch and configuration files the preferred method for starting any of its nodes. The package contains template launch and configuration files to help get users started.
Parameters Common to *ekf_localization_node* and *ukf_localization_node*
========================================================================
Standard Parameters
-------------------
~frequency
^^^^^^^^^^
The real-valued frequency, in Hz, at which the filter produces a state estimate.
.. note:: The filter will not begin computation until it receives at least one message from one of the inputs.
~sensor_timeout
^^^^^^^^^^^^^^^
The real-valued period, in seconds, after which we consider any sensor to have timed out. In this event, we carry out a predict cycle on the EKF without correcting it. This parameter can be thought of as the inverse of the minimum frequency at which the filter will generate *new* output.
~two_d_mode
^^^^^^^^^^^
If your robot is operating in a planar environment and you're comfortable with ignoring the subtle variations in the ground (as reported by an IMU), then set this to true. It will fuse 0 values for all 3D variables (Z, roll, pitch, and their respective velocities and accelerations). This keeps the covariances for those values from exploding while ensuring that your robot's state estimate remains affixed to the X-Y plane.
~[frame]
^^^^^^^^^
Specific parameters:
* ``~map_frame``
* ``~odom_frame``
* ``~base_link_frame``
* ``~base_link_frame_output``
* ``~world_frame``
These parameters define the operating "mode" for ``robot_localization``. `REP-105 <http://www.ros.org/reps/rep-0105.html>`_ specifies three principal coordinate frames: *map*, *odom*, and *base_link*. *base_link* is the coordinate frame that is affixed to the robot. The robot's position in the *odom* frame will drift over time, but is accurate in the short term and should be continuous. The *map* frame, like the *odom* frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data. Here is how to use these parameters:
1. Set the ``map_frame``, ``odom_frame``, and ``base_link_frame`` parameters to the appropriate frame names for your system.
.. note:: If your system does not have a ``map_frame``, just remove it, and make sure ``world_frame`` is set to the value of ``odom_frame``.
.. note:: If you are running multiple EKF instances and would like to "override" the output transform and message to have this frame for its ``child_frame_id``, you may set this. The ``base_link_frame_output`` is optional and will default to the ``base_link_frame``. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we "override" the output transform and message to have this frame for its ``child_frame_id``.
2. If you are only fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set ``world_frame`` to your ``odom_frame`` value. This is the default behavior for the state estimation nodes in ``robot_localization``, and the most common use for it.
3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then:
i. Set your ``world_frame`` to your ``map_frame`` value
ii. **Make sure** something else is generating the *odom->base_link* transform. This can even be another instance of a ``robot_localization`` state estimation node. However, that instance should *not* fuse the global data.
The default values for ``map_frame``, ``odom_frame``, and ``base_link_frame`` are *map*, *odom,* and *base_link,* respectively. The ``base_link_frame_output`` parameter defaults to the value of ``base_link_frame``. The ``world_frame`` parameter defaults to the value of ``odom_frame``.
~transform_time_offset
^^^^^^^^^^^^^^^^^^^^^^
Some packages require that your transforms are future-dated by a small time offset. The value of this parameter will be added to the timestamp of *map->odom* or *odom->base_link* transform being generated by the state estimation nodes in ``robot_localization``.
~transform_timeout
^^^^^^^^^^^^^^^^^^
The ``robot_localization`` package uses ``tf2``'s ``lookupTransform`` method to request transformations. This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see ``tf2`` implementation) transform so we are not blocking the filter. Specifying a non-zero `transform_timeout` affects the filter's timing since it waits for a maximum of the `transform_timeout` for a transform to become available. This directly implies that mostly the specified desired output rate is not met since the filter has to wait for transforms when updating.
~[sensor]
^^^^^^^^^
For each sensor, users need to define this parameter based on the message type. For example, if we define one source of Imu messages and two sources of Odometry messages, the configuration would look like this:
.. code-block:: xml
<param name="imu0" value="robot/imu/data"/>
<param name="odom0" value="wheel_encoder/odometry"/>
<param name="odom1" value="visual_odometry/odometry"/>
The index for each parameter name is 0-based (e.g., ``odom0``, ``odom1``, etc.) and must be defined sequentially (e.g., do *not* use ``pose0`` and ``pose2`` if you have not defined ``pose1``). The values for each parameter are the topic name for that sensor.
~[sensor]_config
^^^^^^^^^^^^^^^^
Specific parameters:
* ``~odomN_config``
* ``~twistN_config``
* ``~imuN_config``
* ``~poseN_config``
For each of the sensor messages defined above, users must specify what variables of those messages should be fused into the final state estimate. An example odometry configuration might look like this:
.. code-block:: xml
<rosparam param="odom0_config">[true, true, false,
false, false, true,
true, false, false,
false, false, true,
false, false, false]</rosparam>
The order of the boolean values are :math:`X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z}`. In this example, we are fusing :math:`X` and :math:`Y` position, :math:`yaw`, :math:`\dot{X}`, and :math:`\dot{yaw}`.
.. note:: The specification is done in the ``frame_id`` of the **sensor**, *not* in the ``world_frame`` or ``base_link_frame``. Please see the :doc:`coniguration tutorial <configuring_robot_localization>` for more information.
~[sensor]_queue_size
^^^^^^^^^^^^^^^^^^^^
Specific parameters:
* ``~odomN_queue_size``
* ``~twistN_queue_size``
* ``~imuN_queue_size``
* ``~poseN_queue_size``
Users can use these parameters to adjust the callback queue sizes for each sensor. This is useful if your ``frequency`` parameter value is much lower than your sensor's frequency, as it allows the filter to incorporate all measurements that arrived in between update cycles.
~[sensor]_differential
^^^^^^^^^^^^^^^^^^^^^^
Specific parameters:
* ``~odomN_differential``
* ``~imuN_differential``
* ``~poseN_differential``
For each of the sensor messages defined above *that contain pose information*, users can specify whether the pose variables should be integrated differentially. If a given value is set to *true*, then for a measurement at time :math:`t` from the sensor in question, we first subtract the measurement at time :math:`t-1`, and convert the resulting value to a velocity. This setting is especially useful if your robot has two sources of absolute pose information, e.g., yaw measurements from odometry and an IMU. In that case, if the variances on the input sources are not configured correctly, these measurements may get out of sync with one another and cause oscillations in the filter, but by integrating one or both of them differentially, we avoid this scenario.
Users should take care when using this parameter for orientation data, as the conversion to velocity means that the covariance for orientation state variables will grow without bound (unless another source of absolute orientation data is being fused). If you simply want all of your pose variables to start at :math:`0`, then please use the ``_relative`` parameter.
.. note:: If you are fusing GPS information via ``navsat_transform_node`` or ``utm_transform_node``, you should make sure that the ``_differential`` setting is *false.*
~[sensor]_relative
^^^^^^^^^^^^^^^^^^
Specific parameters:
* ``~odomN_relative``
* ``~imuN_relative``
* ``~poseN_relative``
If this parameter is set to ``true``, then any measurements from this sensor will be fused relative to the first measurement received from that sensor. This is useful if, for example, you want your state estimate to always start at :math:`(0, 0, 0)` and with :math:`roll, pitch,` and :math:`yaw` values of :math:`(0, 0, 0)`. It is similar to the ``_differential`` parameter, but instead of removing the measurement at time :math:`t-1`, we always remove the measurement at time :math:`0`, and the measurement is not converted to a velocity.
~imuN_remove_gravitational_acceleration
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
If fusing accelerometer data from IMUs, this parameter determines whether or not acceleration due to gravity is removed from the acceleration measurement before fusing it.
.. note:: This assumes that the IMU that is providing the acceleration data is also producing an absolute orientation. The orientation data is required to correctly remove gravitational acceleration.
~gravitational_acceleration
^^^^^^^^^^^^^^^^^^^^^^^^^^^
If ``imuN_remove_gravitational_acceleration`` is set to ``true``, then this parameter determines the acceleration in Z due to gravity that will be removed from the IMU's linear acceleration data. Default is 9.80665 (m/s^2).
~initial_state
^^^^^^^^^^^^^^
Starts the filter with the specified state. The state is given as a 15-D vector of doubles, in the same order as the sensor configurations. For example, to start your robot at a position of :math:`(5.0, 4.0, 3.0)`, a :math:`yaw` of :math:`1.57`, and a linear velocity of :math:`(0.1, 0.2, 0.3)`, you would use:
.. code-block:: xml
<rosparam param="initial_state">[5.0, 4.0, 3.0,
0.0, 0.0, 1.57,
0.1, 0.2, 0.3,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]</rosparam>
~publish_tf
^^^^^^^^^^^
If *true*, the state estimation node will publish the transform from the frame specified by the ``world_frame`` parameter to the frame specified by the ``base_link_frame`` parameter. Defaults to *true*.
~publish_acceleration
^^^^^^^^^^^^^^^^^^^^^
If *true*, the state estimation node will publish the linear acceleration state. Defaults to *false*.
~permit_corrected_publication
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
When the state estimation nodes publish the state at time `t`, but then receive a measurement with a timestamp < `t`, they re-publish the corrected state, with the same time stamp as the previous publication. Setting this parameter to *false* disables that behavior. Defaults to *false*.
~print_diagnostics
^^^^^^^^^^^^^^^^^^
If true, the state estimation node will publish diagnostic messages to the ``/diagnostics`` topic. This is useful for debugging your configuration and sensor data.
Advanced Parameters
-------------------
~use_control
^^^^^^^^^^^^
If *true*, the state estimation node will listen to the `cmd_vel` topic for a `geometry_msgs/Twist <http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html>`_ message, and use that to generate an acceleration term. This term is then used in the robot's state prediction. This is especially useful in situations where even small amounts of lag in convergence for a given state variable cause problems in your application (e.g., LIDAR shifting during rotations). Defaults to *false*.
.. note:: The presence and inclusion of linear acceleration data from an IMU will currently "override" the predicted linear acceleration value.
~stamped_control
^^^^^^^^^^^^^^^^
If *true* and ``use_control`` is also *true*, looks for a `geometry_msgs/TwistStamped <http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html>`_ message instead of a `geometry_msgs/Twist <http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html>`_ message.
~control_timeout
^^^^^^^^^^^^^^^^
If ``use_control`` is set to *true* and no control command is received in this amount of time, given in seconds, the control-based acceleration term ceases to be applied.
~control_config
^^^^^^^^^^^^^^^
Controls which variables in the ``cmd_vel`` message are used in state prediction. The order of the values is :math:`\dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}`. Only used if ``use_control`` is set to *true*.
.. code-block:: xml
<rosparam param="control_config">[true, false, false,
false, false, true]</rosparam>
~acceleration_limits
^^^^^^^^^^^^^^^^^^^^
How rapidly your robot can accelerate for each dimension. Matches the parameter order in ``control_config``. Only used if ``use_control`` is set to *true*.
.. code-block:: xml
<rosparam param="acceleration_limits">[1.3, 0.0, 0.0,
0.0, 0.0, 3.2]</rosparam>
~deceleration_limits
^^^^^^^^^^^^^^^^^^^^
How rapidly your robot can decelerate for each dimension. Matches the parameter order in ``control_config``. Only used if ``use_control`` is set to *true*.
~acceleration_gains
^^^^^^^^^^^^^^^^^^^
If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these gains. Only used if ``use_control`` is set to *true*.
.. code-block:: xml
<rosparam param="acceleration_limits">[0.8, 0.0, 0.0,
0.0, 0.0, 0.9]</rosparam>
~deceleration_gains
^^^^^^^^^^^^^^^^^^^
If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these gains. Only used if ``use_control`` is set to *true*.
~smooth_lagged_data
^^^^^^^^^^^^^^^^^^^
If any of your sensors produce data with timestamps that are older than the most recent filter update (more plainly, if you have a source of lagged sensor data), setting this parameter to *true* will enable the filter, upon reception of lagged data, to revert to the last state prior to the lagged measurement, then process all measurements until the current time. This is especially useful for measurements that come from nodes that require heavy CPU usage to generate pose estimates (e.g., laser scan matchers), as they are frequently lagged behind the current time.
~history_length
^^^^^^^^^^^^^^^
If ``smooth_lagged_data`` is set to *true*, this parameter specifies the number of seconds for which the filter will retain its state and measurement history. This value should be at least as large as the time delta between your lagged measurements and the current time.
~[sensor]_nodelay
^^^^^^^^^^^^^^^^^
Specific parameters:
* ``~odomN_nodelay``
* ``~twistN_nodelay``
* ``~imuN_nodelay``
* ``~poseN_nodelay``
If *true*, sets the `tcpNoDelay` `transport hint <http://docs.ros.org/api/roscpp/html/classros_1_1TransportHints.html#a03191a9987162fca0ae2c81fa79fcde9>`_. There is some evidence that Nagle's algorithm intereferes with the timely reception of large message types, such as the `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message. Setting this to *true* for an input disables Nagle's algorithm for that subscriber. Defaults to *false*.
~[sensor]_threshold
^^^^^^^^^^^^^^^^^^^
Specific parameters:
* ``~odomN_pose_rejection_threshold``
* ``odomN_twist_rejection_threshold``
* ``poseN_rejection_threshold``
* ``twistN_rejection_threshold``
* ``imuN_pose_rejection_threshold``
* ``imuN_angular_velocity_rejection_threshold``
* ``imuN_linear_acceleration_rejection_threshold``
If your data is subject to outliers, use these threshold settings, expressed as `Mahalanobis distances <http://en.wikipedia.org/wiki/Mahalanobis_distance>`_, to control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to ``numeric_limits<double>::max()`` if unspecified.
~debug
^^^^^^
Boolean flag that specifies whether or not to run in debug mode. WARNING: setting this to true will generate a massive amount of data. The data is written to the value of the ``debug_out_file`` parameter. Defaults to *false*.
~debug_out_file
^^^^^^^^^^^^^^^^
If ``debug`` is *true*, the file to which debug output is written.
~process_noise_covariance
^^^^^^^^^^^^^^^^^^^^^^^^^
The process noise covariance, commonly denoted *Q*, is used to model uncertainty in the prediction stage of the filtering algorithms. It can be difficult to tune, and has been exposed as a parameter for easier customization. This parameter can be left alone, but you will achieve superior results by tuning it. In general, the larger the value for *Q* relative to the variance for a given variable in an input message, the faster the filter will converge to the value in the measurement.
~dynamic_process_noise_covariance
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
If *true*, will dynamically scale the ``process_noise_covariance`` based on the robot's velocity. This is useful, e.g., when you want your robot's estimate error covariance to stop growing when the robot is stationary. Defaults to *false*.
~initial_estimate_covariance
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
The estimate covariance, commonly denoted *P*, defines the error in the current state estimate. The parameter allows users to set the initial value for the matrix, which will affect how quickly the filter converges. For example, if users set the value at position :math:`[0, 0]` to a very small value, e.g., `1e-12`, and then attempt to fuse measurements of X position with a high variance value for :math:`X`, then the filter will be very slow to "trust" those measurements, and the time required for convergence will increase. Again, users should take care with this parameter. When only fusing velocity data (e.g., no absolute pose information), users will likely *not* want to set the initial covariance values for the absolute pose variables to large numbers. This is because those errors are going to grow without bound (owing to the lack of absolute pose measurements to reduce the error), and starting them with large values will not benefit the state estimate.
~reset_on_time_jump
^^^^^^^^^^^^^^^^^^^
If set to *true* and ``ros::Time::isSimTime()`` is *true*, the filter will reset to its uninitialized state when a jump back in time is detected on a topic. This is useful when working with bag data, in that the bag can be restarted without restarting the node.
~predict_to_current_time
^^^^^^^^^^^^^^^^^^^^^^^^
If set to *true*, the filter predicts and corrects up to the time of the latest measurement (by default) but will now also predict up to the current time step.
~disabled_at_startup
^^^^^^^^^^^^^^^^^^^^
If set to *true* will not run the filter on start.
Node-specific Parameters
------------------------
The standard and advanced parameters are common to all state estimation nodes in ``robot_localization``. This section details parameters that are unique to their respective state estimation nodes.
ukf_localization_node
^^^^^^^^^^^^^^^^^^^^^
The parameters for ``ukf_localization_node`` follow the nomenclature of the `original paper <http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=882463&tag=1>`_ and `wiki article <http://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter>`_.
* **~alpha** - Controls the spread of sigma points. Unless you are familiar with unscented Kalman filters, it's probably best for this setting to remain at its default value (0.001).
* **~kappa** - Also control the spread of sigma points. Unless you are familiar with unscented Kalman filters, it's probably best for this setting to remain at its default value (0).
* **~beta** - Relates to the distribution of the state vector. The default value of 2 implies that the distribution is Gaussian. Like the other parameters, this should remain unchanged unless the user is familiar with unscented Kalman filters.
Published Topics
================
* ``odometry/filtered`` (`nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_)
* ``accel/filtered`` (`geometry_msgs/AccelWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/AccelWithCovarianceStamped.html>`_) (if enabled)
Published Transforms
====================
* If the user's ``world_frame`` parameter is set to the value of ``odom_frame``, a transform is published from the frame given by the ``odom_frame`` parameter to the frame given by the ``base_link_frame`` parameter.
* If the user's ``world_frame`` parameter is set to the value of ``map_frame``, a transform is published from the frame given by the ``map_frame`` parameter to the frame given by the ``odom_frame`` parameter.
.. note:: This mode assumes that another node is broadcasting the transform from the frame given by the ``odom_frame`` parameter to the frame given by the ``base_link_frame`` parameter. This can be another instance of a ``robot_localization`` state estimation node.
Services
========
* ``set_pose`` - By issuing a `geometry_msgs/PoseWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html>`_ message to the ``set_pose`` topic, users can manually set the state of the filter. This is useful for resetting the filter during testing, and allows for interaction with ``rviz``. Alternatively, the state estimation nodes advertise a ``SetPose`` service, whose type is `robot_localization/SetPose <http://docs.ros.org/api/robot_localization/html/srv/SetPose.html>`_.

13
Localizations/Packages/robot_localization/doc/user_contributed_tutorials.rst Normal file
View File

@@ -0,0 +1,13 @@
User-Contributed Tutorials
##########################
Here's a list of user-contributed tutorials for robot_localization!
Tutorials
=========
* `ros-sensor-fusion-tutorial <https://github.com/methylDragon/ros-sensor-fusion-tutorial>`_
A comprehensive end-to-end tutorial for setting up robot_localization for sensor fusion, as well as running through the necessary concepts. (Includes a practical example of setting it up with ultrasonic beacons!)
* `Kapernikov: The ROS robot_localization package <https://kapernikov.com/the-ros-robot_localization-package/>`_
The documentation of the robot_localization package is quite clear once you know how it works. However, it lacks a hands-on tutorial to help you with your first steps. There are some great examples on how to set up the robot_localization package, but they require good working hardware. This tutorial tries to bridge the gap, using the turtlesim package as a virtual robot.

87
Localizations/Packages/robot_localization/include/robot_localization/ekf.h Normal file
View File

@@ -0,0 +1,87 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_EKF_H
#define ROBOT_LOCALIZATION_EKF_H
#include "robot_localization/filter_base.h"
#include <fstream>
#include <vector>
#include <set>
#include <queue>
namespace RobotLocalization
{
//! @brief Extended Kalman filter class
//!
//! Implementation of an extended Kalman filter (EKF). This
//! class derives from FilterBase and overrides the predict()
//! and correct() methods in keeping with the discrete time
//! EKF algorithm.
//!
class Ekf: public FilterBase
{
public:
//! @brief Constructor for the Ekf class
//!
//! @param[in] args - Generic argument container (not used here, but
//! needed so that the ROS filters can pass arbitrary arguments to
//! templated filter types).
//!
explicit Ekf(std::vector<double> args = std::vector<double>());
//! @brief Destructor for the Ekf class
//!
~Ekf();
//! @brief Carries out the correct step in the predict/update cycle.
//!
//! @param[in] measurement - The measurement to fuse with our estimate
//!
void correct(const Measurement &measurement);
//! @brief Carries out the predict step in the predict/update cycle.
//!
//! Projects the state and error matrices forward using a model of
//! the vehicle's motion.
//!
//! @param[in] referenceTime - The time at which the prediction is being made
//! @param[in] delta - The time step over which to predict.
//!
void predict(const double referenceTime, const double delta);
};
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_EKF_H

534
Localizations/Packages/robot_localization/include/robot_localization/filter_base.h Normal file
View File

@@ -0,0 +1,534 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_FILTER_BASE_H
#define ROBOT_LOCALIZATION_FILTER_BASE_H
#include "robot_localization/filter_utilities.h"
#include "robot_localization/filter_common.h"
#include <Eigen/Dense>
#include <algorithm>
#include <limits>
#include <map>
#include <ostream>
#include <queue>
#include <set>
#include <string>
#include <vector>
#include <boost/shared_ptr.hpp>
namespace RobotLocalization
{
//! @brief Structure used for storing and comparing measurements
//! (for priority queues)
//!
//! Measurement units are assumed to be in meters and radians.
//! Times are real-valued and measured in seconds.
//!
struct Measurement
{
// The time stamp of the most recent control term (needed for lagged data)
double latestControlTime_;
// The Mahalanobis distance threshold in number of sigmas
double mahalanobisThresh_;
// The real-valued time, in seconds, since some epoch
// (presumably the start of execution, but any will do)
double time_;
// The topic name for this measurement. Needed
// for capturing previous state values for new
// measurements.
std::string topicName_;
// This defines which variables within this measurement
// actually get passed into the filter. std::vector<bool>
// is generally frowned upon, so we use ints.
std::vector<int> updateVector_;
// The most recent control vector (needed for lagged data)
Eigen::VectorXd latestControl_;
// The measurement and its associated covariance
Eigen::VectorXd measurement_;
Eigen::MatrixXd covariance_;
// We want earlier times to have greater priority
bool operator()(const boost::shared_ptr<Measurement> &a, const boost::shared_ptr<Measurement> &b)
{
return (*this)(*(a.get()), *(b.get()));
}
bool operator()(const Measurement &a, const Measurement &b)
{
return a.time_ > b.time_;
}
Measurement() :
latestControlTime_(0.0),
mahalanobisThresh_(std::numeric_limits<double>::max()),
time_(0.0),
topicName_("")
{
}
};
typedef boost::shared_ptr<Measurement> MeasurementPtr;
//! @brief Structure used for storing and comparing filter states
//!
//! This structure is useful when higher-level classes need to remember filter history.
//! Measurement units are assumed to be in meters and radians.
//! Times are real-valued and measured in seconds.
//!
struct FilterState
{
// The time stamp of the most recent measurement for the filter
double lastMeasurementTime_;
// The time stamp of the most recent control term
double latestControlTime_;
// The most recent control vector
Eigen::VectorXd latestControl_;
// The filter state vector
Eigen::VectorXd state_;
// The filter error covariance matrix
Eigen::MatrixXd estimateErrorCovariance_;
// We want the queue to be sorted from latest to earliest timestamps.
bool operator()(const FilterState &a, const FilterState &b)
{
return a.lastMeasurementTime_ < b.lastMeasurementTime_;
}
FilterState() :
lastMeasurementTime_(0.0),
latestControlTime_(0.0)
{}
};
typedef boost::shared_ptr<FilterState> FilterStatePtr;
class FilterBase
{
public:
//! @brief Constructor for the FilterBase class
//!
FilterBase();
//! @brief Destructor for the FilterBase class
//!
virtual ~FilterBase();
//! @brief Resets filter to its unintialized state
//!
void reset();
//! @brief Computes a dynamic process noise covariance matrix using the parameterized state
//!
//! This allows us to, e.g., not increase the pose covariance values when the vehicle is not moving
//!
//! @param[in] state - The STATE_SIZE state vector that is used to generate the dynamic process noise covariance
//!
void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta);
//! @brief Carries out the correct step in the predict/update cycle. This method
//! must be implemented by subclasses.
//!
//! @param[in] measurement - The measurement to fuse with the state estimate
//!
virtual void correct(const Measurement &measurement) = 0;
//! @brief Returns the control vector currently being used
//!
//! @return The control vector
//!
const Eigen::VectorXd& getControl();
//! @brief Returns the time at which the control term was issued
//!
//! @return The time the control vector was issued
//!
double getControlTime();
//! @brief Gets the value of the debug_ variable.
//!
//! @return True if in debug mode, false otherwise
//!
bool getDebug();
//! @brief Gets the estimate error covariance
//!
//! @return A copy of the estimate error covariance matrix
//!
const Eigen::MatrixXd& getEstimateErrorCovariance();
//! @brief Gets the filter's initialized status
//!
//! @return True if we've received our first measurement, false otherwise
//!
bool getInitializedStatus();
//! @brief Gets the most recent measurement time
//!
//! @return The time at which we last received a measurement
//!
double getLastMeasurementTime();
//! @brief Gets the filter's predicted state, i.e., the
//! state estimate before correct() is called.
//!
//! @return A constant reference to the predicted state
//!
const Eigen::VectorXd& getPredictedState();
//! @brief Gets the filter's process noise covariance
//!
//! @return A constant reference to the process noise covariance
//!
const Eigen::MatrixXd& getProcessNoiseCovariance();
//! @brief Gets the sensor timeout value (in seconds)
//!
//! @return The sensor timeout value
//!
double getSensorTimeout();
//! @brief Gets the filter state
//!
//! @return A constant reference to the current state
//!
const Eigen::VectorXd& getState();
//! @brief Carries out the predict step in the predict/update cycle.
//! Projects the state and error matrices forward using a model of
//! the vehicle's motion. This method must be implemented by subclasses.
//!
//! @param[in] referenceTime - The time at which the prediction is being made
//! @param[in] delta - The time step over which to predict.
//!
virtual void predict(const double referenceTime, const double delta) = 0;
//! @brief Does some final preprocessing, carries out the predict/update cycle
//!
//! @param[in] measurement - The measurement object to fuse into the filter
//!
virtual void processMeasurement(const Measurement &measurement);
//! @brief Sets the most recent control term
//!
//! @param[in] control - The control term to be applied
//! @param[in] controlTime - The time at which the control in question was received
//!
void setControl(const Eigen::VectorXd &control, const double controlTime);
//! @brief Sets the control update vector and acceleration limits
//!
//! @param[in] updateVector - The values the control term affects
//! @param[in] controlTimeout - Timeout value, in seconds, after which a control is considered stale
//! @param[in] accelerationLimits - The acceleration limits for the control variables
//! @param[in] accelerationGains - Gains applied to the control term-derived acceleration
//! @param[in] decelerationLimits - The deceleration limits for the control variables
//! @param[in] decelerationGains - Gains applied to the control term-derived deceleration
//!
void setControlParams(const std::vector<int> &updateVector, const double controlTimeout,
const std::vector<double> &accelerationLimits, const std::vector<double> &accelerationGains,
const std::vector<double> &decelerationLimits, const std::vector<double> &decelerationGains);
//! @brief Sets the filter into debug mode
//!
//! NOTE: this will generates a lot of debug output to the provided stream.
//! The value must be a pointer to a valid ostream object.
//!
//! @param[in] debug - Whether or not to place the filter in debug mode
//! @param[in] outStream - If debug is true, then this must have a valid pointer.
//! If the pointer is invalid, the filter will not enter debug mode. If debug is
//! false, outStream is ignored.
//!
void setDebug(const bool debug, std::ostream *outStream = NULL);
//! @brief Enables dynamic process noise covariance calculation
//!
//! @param[in] dynamicProcessNoiseCovariance - Whether or not to compute dynamic process noise covariance matrices
//!
void setUseDynamicProcessNoiseCovariance(const bool dynamicProcessNoiseCovariance);
//! @brief Manually sets the filter's estimate error covariance
//!
//! @param[in] estimateErrorCovariance - The state to set as the filter's current state
//!
void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance);
//! @brief Sets the filter's last measurement time.
//!
//! @param[in] lastMeasurementTime - The last measurement time of the filter
//!
void setLastMeasurementTime(const double lastMeasurementTime);
//! @brief Sets the process noise covariance for the filter.
//!
//! This enables external initialization, which is important, as this
//! matrix can be difficult to tune for a given implementation.
//!
//! @param[in] processNoiseCovariance - The STATE_SIZExSTATE_SIZE process noise covariance matrix
//! to use for the filter
//!
void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance);
//! @brief Sets the sensor timeout
//!
//! @param[in] sensorTimeout - The time, in seconds, for a sensor to be
//! considered having timed out
//!
void setSensorTimeout(const double sensorTimeout);
//! @brief Manually sets the filter's state
//!
//! @param[in] state - The state to set as the filter's current state
//!
void setState(const Eigen::VectorXd &state);
//! @brief Ensures a given time delta is valid (helps with bag file playback issues)
//!
//! @param[in] delta - The time delta, in seconds, to validate
//!
void validateDelta(double &delta);
protected:
//! @brief Method for settings bounds on acceleration values derived from controls
//! @param[in] state - The current state variable (e.g., linear X velocity)
//! @param[in] control - The current control commanded velocity corresponding to the state variable
//! @param[in] accelerationLimit - Limit for acceleration (regardless of driving direction)
//! @param[in] accelerationGain - Gain applied to acceleration control error
//! @param[in] decelerationLimit - Limit for deceleration (moving towards zero, regardless of driving direction)
//! @param[in] decelerationGain - Gain applied to deceleration control error
//! @return a usable acceleration estimate for the control vector
//!
inline double computeControlAcceleration(const double state, const double control, const double accelerationLimit,
const double accelerationGain, const double decelerationLimit, const double decelerationGain)
{
FB_DEBUG("---------- FilterBase::computeControlAcceleration ----------\n");
const double error = control - state;
const bool sameSign = (::fabs(error) <= ::fabs(control) + 0.01);
const double setPoint = (sameSign ? control : 0.0);
const bool decelerating = ::fabs(setPoint) < ::fabs(state);
double limit = accelerationLimit;
double gain = accelerationGain;
if (decelerating)
{
limit = decelerationLimit;
gain = decelerationGain;
}
const double finalAccel = std::min(std::max(gain * error, -limit), limit);
FB_DEBUG("Control value: " << control << "\n" <<
"State value: " << state << "\n" <<
"Error: " << error << "\n" <<
"Same sign: " << (sameSign ? "true" : "false") << "\n" <<
"Set point: " << setPoint << "\n" <<
"Decelerating: " << (decelerating ? "true" : "false") << "\n" <<
"Limit: " << limit << "\n" <<
"Gain: " << gain << "\n" <<
"Final is " << finalAccel << "\n");
return finalAccel;
}
//! @brief Keeps the state Euler angles in the range [-pi, pi]
//!
virtual void wrapStateAngles();
//! @brief Tests if innovation is within N-sigmas of covariance. Returns true if passed the test.
//! @param[in] innovation - The difference between the measurement and the state
//! @param[in] invCovariance - The innovation error
//! @param[in] nsigmas - Number of standard deviations that are considered acceptable
//!
virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
const Eigen::MatrixXd &invCovariance,
const double nsigmas);
//! @brief Converts the control term to an acceleration to be applied in the prediction step
//! @param[in] referenceTime - The time of the update (measurement used in the prediction step)
//! @param[in] predictionDelta - The amount of time over which we are carrying out our prediction
//!
void prepareControl(const double referenceTime, const double predictionDelta);
//! @brief Whether or not we've received any measurements
//!
bool initialized_;
//! @brief Whether or not we apply the control term
//!
bool useControl_;
//! @brief If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a
//! dynamic process noise covariance matrix
//!
bool useDynamicProcessNoiseCovariance_;
//! @brief Tracks the time the filter was last updated using a measurement.
//!
//! This value is used to monitor sensor readings with respect to the sensorTimeout_.
//! We also use it to compute the time delta values for our prediction step.
//!
double lastMeasurementTime_;
//! @brief The time of reception of the most recent control term
//!
double latestControlTime_;
//! @brief Timeout value, in seconds, after which a control is considered stale
//!
double controlTimeout_;
//! @brief The updates to the filter - both predict and correct - are driven
//! by measurements. If we get a gap in measurements for some reason, we want
//! the filter to continue estimating. When this gap occurs, as specified by
//! this timeout, we will continue to call predict() at the filter's frequency.
//!
double sensorTimeout_;
//! @brief Which control variables are being used (e.g., not every vehicle is controllable in Y or Z)
//!
std::vector<int> controlUpdateVector_;
//! @brief Gains applied to acceleration derived from control term
//!
std::vector<double> accelerationGains_;
//! @brief Caps the acceleration we apply from control input
//!
std::vector<double> accelerationLimits_;
//! @brief Gains applied to deceleration derived from control term
//!
std::vector<double> decelerationGains_;
//! @brief Caps the deceleration we apply from control input
//!
std::vector<double> decelerationLimits_;
//! @brief Variable that gets updated every time we process a measurement and we have a valid control
//!
Eigen::VectorXd controlAcceleration_;
//! @brief Latest control term
//!
Eigen::VectorXd latestControl_;
//! @brief Holds the last predicted state of the filter
//!
Eigen::VectorXd predictedState_;
//! @brief This is the robot's state vector, which is what we are trying to
//! filter. The values in this vector are what get reported by the node.
//!
Eigen::VectorXd state_;
//! @brief Covariance matrices can be incredibly unstable. We can
//! add a small value to it at each iteration to help maintain its
//! positive-definite property.
//!
Eigen::MatrixXd covarianceEpsilon_;
//! @brief Gets updated when useDynamicProcessNoise_ is true
//!
Eigen::MatrixXd dynamicProcessNoiseCovariance_;
//! @brief This matrix stores the total error in our position
//! estimate (the state_ variable).
//!
Eigen::MatrixXd estimateErrorCovariance_;
//! @brief We need the identity for a few operations. Better to store it.
//!
Eigen::MatrixXd identity_;
//! @brief As we move through the world, we follow a predict/update
//! cycle. If one were to imagine a scenario where all we did was make
//! predictions without correcting, the error in our position estimate
//! would grow without bound. This error is stored in the
//! stateEstimateCovariance_ matrix. However, this matrix doesn't answer
//! the question of *how much* our error should grow for each time step.
//! That's where the processNoiseCovariance matrix comes in. When we
//! make a prediction using the transfer function, we add this matrix
//! (times deltaT) to the state estimate covariance matrix.
//!
Eigen::MatrixXd processNoiseCovariance_;
//! @brief The Kalman filter transfer function
//!
//! Kalman filters and extended Kalman filters project the current
//! state forward in time. This is the "predict" part of the predict/correct
//! cycle. A Kalman filter has a (typically constant) matrix A that defines
//! how to turn the current state, x, into the predicted next state. For an
//! EKF, this matrix becomes a function f(x). However, this function can still
//! be expressed as a matrix to make the math a little cleaner, which is what
//! we do here. Technically, each row in the matrix is actually a function.
//! Some rows will contain many trigonometric functions, which are of course
//! non-linear. In any case, you can think of this as the 'A' matrix in the
//! Kalman filter formulation.
//!
Eigen::MatrixXd transferFunction_;
//! @brief The Kalman filter transfer function Jacobian
//!
//! The transfer function is allowed to be non-linear in an EKF, but
//! for propagating (predicting) the covariance matrix, we need to linearize
//! it about the current mean (i.e., state). This is done via a Jacobian,
//! which calculates partial derivatives of each row of the transfer function
//! matrix with respect to each state variable.
//!
Eigen::MatrixXd transferFunctionJacobian_;
//! @brief Used for outputting debug messages
//!
std::ostream *debugStream_;
private:
//! @brief Whether or not the filter is in debug mode
//!
bool debug_;
};
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_FILTER_BASE_H

97
Localizations/Packages/robot_localization/include/robot_localization/filter_common.h Normal file
View File

@@ -0,0 +1,97 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_FILTER_COMMON_H
#define ROBOT_LOCALIZATION_FILTER_COMMON_H
namespace RobotLocalization
{
//! @brief Enumeration that defines the state vector
//!
enum StateMembers
{
StateMemberX = 0,
StateMemberY,
StateMemberZ,
StateMemberRoll,
StateMemberPitch,
StateMemberYaw,
StateMemberVx,
StateMemberVy,
StateMemberVz,
StateMemberVroll,
StateMemberVpitch,
StateMemberVyaw,
StateMemberAx,
StateMemberAy,
StateMemberAz
};
//! @brief Enumeration that defines the control vector
//!
enum ControlMembers
{
ControlMemberVx,
ControlMemberVy,
ControlMemberVz,
ControlMemberVroll,
ControlMemberVpitch,
ControlMemberVyaw
};
//! @brief Global constants that define our state
//! vector size and offsets to groups of values
//! within that state.
const int STATE_SIZE = 15;
const int POSITION_OFFSET = StateMemberX;
const int ORIENTATION_OFFSET = StateMemberRoll;
const int POSITION_V_OFFSET = StateMemberVx;
const int ORIENTATION_V_OFFSET = StateMemberVroll;
const int POSITION_A_OFFSET = StateMemberAx;
//! @brief Pose and twist messages each
//! contain six variables
const int POSE_SIZE = 6;
const int TWIST_SIZE = 6;
const int POSITION_SIZE = 3;
const int ORIENTATION_SIZE = 3;
const int LINEAR_VELOCITY_SIZE = 3;
const int ACCELERATION_SIZE = 3;
//! @brief Common variables
const double PI = 3.141592653589793;
const double TAU = 6.283185307179587;
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_FILTER_COMMON_H

71
Localizations/Packages/robot_localization/include/robot_localization/filter_utilities.h Normal file
View File

@@ -0,0 +1,71 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_FILTER_UTILITIES_H
#define ROBOT_LOCALIZATION_FILTER_UTILITIES_H
#include <Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#define FB_DEBUG(msg) if (getDebug()) { *debugStream_ << msg; }
// Handy methods for debug output
std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat);
std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec);
std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec);
std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec);
namespace RobotLocalization
{
namespace FilterUtilities
{
//! @brief Utility method keeping RPY angles in the range [-pi, pi]
//! @param[in] rotation - The rotation to bind
//! @return the bounded value
//!
double clampRotation(double rotation);
//! @brief Utility method for appending tf2 prefixes cleanly
//! @param[in] tfPrefix - the tf2 prefix to append
//! @param[in, out] frameId - the resulting frame_id value
//!
void appendPrefix(std::string tfPrefix, std::string &frameId);
} // namespace FilterUtilities
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_FILTER_UTILITIES_H

216
Localizations/Packages/robot_localization/include/robot_localization/navsat_conversions.h Normal file
View File

@@ -0,0 +1,216 @@
/*
* Copyright (C) 2010 Austin Robot Technology, and others
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the names of the University of Texas at Austin, nor
* Austin Robot Technology, nor the names of other contributors may
* be used to endorse or promote products derived from this
* software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* This file contains code from multiple files in the original
* source. The originals can be found here:
*
* https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/UTM.h
* https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/conversions.h
*/
#ifndef ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H
#define ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H
/** @file
@brief Universal Transverse Mercator transforms.
Functions to convert (spherical) latitude and longitude to and
from (Euclidean) UTM coordinates.
@author Chuck Gantz- chuck.gantz@globalstar.com
*/
#include <cmath>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <GeographicLib/MGRS.hpp>
#include <GeographicLib/UTMUPS.hpp>
namespace RobotLocalization
{
namespace NavsatConversions
{
const double RADIANS_PER_DEGREE = M_PI/180.0;
const double DEGREES_PER_RADIAN = 180.0/M_PI;
// Grid granularity for rounding UTM coordinates to generate MapXY.
const double grid_size = 100000.0; // 100 km grid
// WGS84 Parameters
#define WGS84_A 6378137.0 // major axis
#define WGS84_B 6356752.31424518 // minor axis
#define WGS84_F 0.0033528107 // ellipsoid flattening
#define WGS84_E 0.0818191908 // first eccentricity
#define WGS84_EP 0.0820944379 // second eccentricity
// UTM Parameters
#define UTM_K0 0.9996 // scale factor
#define UTM_FE 500000.0 // false easting
#define UTM_FN_N 0.0 // false northing, northern hemisphere
#define UTM_FN_S 10000000.0 // false northing, southern hemisphere
#define UTM_E2 (WGS84_E*WGS84_E) // e^2
#define UTM_E4 (UTM_E2*UTM_E2) // e^4
#define UTM_E6 (UTM_E4*UTM_E2) // e^6
#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2
/**
* Utility function to convert geodetic to UTM position
*
* Units in are floating point degrees (sign for east/west)
*
* Units out are meters
*
* @todo deprecate this interface in favor of LLtoUTM()
*/
static inline void UTM(double lat, double lon, double *x, double *y)
{
// constants
static const double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
static const double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
static const double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
static const double m3 = -(35*UTM_E6/3072);
// compute the central meridian
int cm = ((lon >= 0.0)
? (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)
: (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));
// convert degrees into radians
double rlat = lat * RADIANS_PER_DEGREE;
double rlon = lon * RADIANS_PER_DEGREE;
double rlon0 = cm * RADIANS_PER_DEGREE;
// compute trigonometric functions
double slat = sin(rlat);
double clat = cos(rlat);
double tlat = tan(rlat);
// decide the false northing at origin
double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
double T = tlat * tlat;
double C = UTM_EP2 * clat * clat;
double A = (rlon - rlon0) * clat;
double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)
+ m2*sin(4*rlat) + m3*sin(6*rlat));
double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);
// compute the easting-northing coordinates
*x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A, 3)/6
+ (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A, 5)/120);
*y = fn + UTM_K0 * (M + V * tlat * (A*A/2
+ (5-T+9*C+4*C*C)*pow(A, 4)/24
+ ((61-58*T+T*T+600*C-330*UTM_EP2)
* pow(A, 6)/720)));
return;
}
/**
* Convert lat/long to UTM coords.
*
* East Longitudes are positive, West longitudes are negative.
* North latitudes are positive, South latitudes are negative
* Lat and Long are in fractional degrees
*
* @param[out] gamma meridian convergence at point (degrees).
*/
static inline void LLtoUTM(const double Lat, const double Long,
double &UTMNorthing, double &UTMEasting,
std::string &UTMZone, double &gamma)
{
int zone;
bool northp;
double k_unused;
GeographicLib::UTMUPS::Forward(Lat, Long, zone, northp, UTMEasting, UTMNorthing, gamma,
k_unused, GeographicLib::UTMUPS::zonespec::MATCH);
GeographicLib::MGRS::Forward(zone, northp, UTMEasting, UTMNorthing, -1, UTMZone);
}
/**
* Convert lat/long to UTM coords.
*
* East Longitudes are positive, West longitudes are negative.
* North latitudes are positive, South latitudes are negative
* Lat and Long are in fractional degrees
*/
static inline void LLtoUTM(const double Lat, const double Long,
double &UTMNorthing, double &UTMEasting,
std::string &UTMZone)
{
double gamma = 0.0;
LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma);
}
/**
* Converts UTM coords to lat/long.
*
* East Longitudes are positive, West longitudes are negative.
* North latitudes are positive, South latitudes are negative
* Lat and Long are in fractional degrees.
*
* @param[out] gamma meridian convergence at point (degrees).
*/
static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
const std::string &UTMZone, double& Lat, double& Long,
double& /*gamma*/)
{
int zone;
bool northp;
double x_unused;
double y_unused;
int prec_unused;
GeographicLib::MGRS::Reverse(UTMZone, zone, northp, x_unused, y_unused, prec_unused, true);
GeographicLib::UTMUPS::Reverse(zone, northp, UTMEasting, UTMNorthing, Lat, Long);
}
/**
* Converts UTM coords to lat/long.
*
* East Longitudes are positive, West longitudes are negative.
* North latitudes are positive, South latitudes are negative
* Lat and Long are in fractional degrees.
*/
static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
const std::string &UTMZone, double& Lat, double& Long)
{
double gamma;
UTMtoLL(UTMNorthing, UTMEasting, UTMZone, Lat, Long, gamma);
}
} // namespace NavsatConversions
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H

382
Localizations/Packages/robot_localization/include/robot_localization/navsat_transform.h Normal file
View File

@@ -0,0 +1,382 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H
#define ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H
#include <robot_localization/SetDatum.h>
#include <robot_localization/ToLL.h>
#include <robot_localization/FromLL.h>
#include <robot_localization/SetUTMZone.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Dense>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <GeographicLib/MGRS.hpp>
#include <GeographicLib/UTMUPS.hpp>
#include <string>
namespace RobotLocalization
{
class NavSatTransform
{
public:
//! @brief Constructor
//!
NavSatTransform(ros::NodeHandle nh, ros::NodeHandle nh_priv);
//! @brief Destructor
//!
~NavSatTransform();
private:
//! @brief callback function which is called for periodic updates
//!
void periodicUpdate(const ros::TimerEvent& event);
//! @brief Computes the transform from the UTM frame to the odom frame
//!
void computeTransform();
//! @brief Callback for the datum service
//!
bool datumCallback(robot_localization::SetDatum::Request& request, robot_localization::SetDatum::Response&);
//! @brief Callback for the to Lat Long service
//!
bool toLLCallback(robot_localization::ToLL::Request& request, robot_localization::ToLL::Response& response);
//! @brief Callback for the from Lat Long service
//!
bool fromLLCallback(robot_localization::FromLL::Request& request, robot_localization::FromLL::Response& response);
//! @brief Callback for the UTM zone service
//!
bool setUTMZoneCallback(robot_localization::SetUTMZone::Request& request,
robot_localization::SetUTMZone::Response& response);
//! @brief Given the pose of the navsat sensor in the cartesian frame, removes the offset from the vehicle's
//! centroid and returns the cartesian-frame pose of said centroid.
//!
void getRobotOriginCartesianPose(const tf2::Transform &gps_cartesian_pose,
tf2::Transform &robot_cartesian_pose,
const ros::Time &transform_time);
//! @brief Given the pose of the navsat sensor in the world frame, removes the offset from the vehicle's centroid
//! and returns the world-frame pose of said centroid.
//!
void getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose,
tf2::Transform &robot_odom_pose,
const ros::Time &transform_time);
//! @brief Callback for the GPS fix data
//! @param[in] msg The NavSatFix message to process
//!
void gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg);
//! @brief Callback for the IMU data
//! @param[in] msg The IMU message to process
//!
void imuCallback(const sensor_msgs::ImuConstPtr& msg);
//! @brief Callback for the odom data
//! @param[in] msg The odometry message to process
//!
void odomCallback(const nav_msgs::OdometryConstPtr& msg);
//! @brief Converts the odometry data back to GPS and broadcasts it
//! @param[out] filtered_gps The NavSatFix message to prepare
//!
bool prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps);
//! @brief Prepares the GPS odometry message before sending
//! @param[out] gps_odom The odometry message to prepare
//!
bool prepareGpsOdometry(nav_msgs::Odometry &gps_odom);
//! @brief Used for setting the GPS data that will be used to compute the transform
//! @param[in] msg The NavSatFix message to use in the transform
//!
void setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg);
//! @brief Used for setting the odometry data that will be used to compute the transform
//! @param[in] msg The odometry message to use in the transform
//!
void setTransformOdometry(const nav_msgs::OdometryConstPtr& msg);
//! @brief Transforms the passed in pose from utm to map frame
//! @param[in] cartesian_pose the pose in cartesian frame to use to transform
//!
nav_msgs::Odometry cartesianToMap(const tf2::Transform& cartesian_pose) const;
//! @brief Transforms the passed in point from map frame to lat/long
//! @param[in] point the point in map frame to use to transform
//!
void mapToLL(const tf2::Vector3& point, double& latitude, double& longitude, double& altitude) const;
//! @brief Whether or not we broadcast the cartesian transform
//!
bool broadcast_cartesian_transform_;
//! @brief Whether to broadcast the cartesian transform as parent frame, default as child
//!
bool broadcast_cartesian_transform_as_parent_frame_;
//! @brief Whether or not we have new GPS data
//!
//! We only want to compute and broadcast our transformed GPS data if it's new. This variable keeps track of that.
//!
bool gps_updated_;
//! @brief Whether or not the GPS fix is usable
//!
bool has_transform_gps_;
//! @brief Signifies that we have received a usable IMU message
//!
bool has_transform_imu_;
//! @brief Signifies that we have received a usable odometry message
//!
bool has_transform_odom_;
//! @brief Whether or not we have new odometry data
//!
//! If we're creating filtered GPS messages, then we only want to broadcast them when new odometry data arrives.
//!
bool odom_updated_;
//! @brief Whether or not we publish filtered GPS messages
//!
bool publish_gps_;
//! @brief Whether or not we've computed a good heading
//!
bool transform_good_;
//! @brief Whether we get our datum from the first GPS message or from the set_datum
//! service/parameter
//!
bool use_manual_datum_;
//! @brief Whether we get the transform's yaw from the odometry or IMU source
//!
bool use_odometry_yaw_;
//! @brief Whether we use a Local Cartesian (tangent plane ENU) or the UTM coordinates as our cartesian
//!
bool use_local_cartesian_;
//! @brief When true, do not print warnings for tf lookup failures.
//!
bool tf_silent_failure_;
//! @brief Local Cartesian projection around gps origin
//!
GeographicLib::LocalCartesian gps_local_cartesian_;
//! @brief Whether or not to report 0 altitude
//!
//! If this parameter is true, we always report 0 for the altitude of the converted GPS odometry message.
//!
bool zero_altitude_;
//! @brief Parameter that specifies the magnetic declination for the robot's environment.
//!
double magnetic_declination_;
//! @brief UTM's meridian convergence
//!
//! Angle between projected meridian (True North) and UTM's grid Y-axis.
//! For UTM projection (Ellipsoidal Transverse Mercator) it is zero on the equator and non-zero everywhere else.
//! It increases as the poles are approached or as we're getting farther from central meridian.
//!
double utm_meridian_convergence_;
//! @brief IMU's yaw offset
//!
//! Your IMU should read 0 when facing *magnetic* north. If it doesn't, this (parameterized) value gives the offset
//! (NOTE: if you have a magenetic declination, use the parameter setting for that).
//!
double yaw_offset_;
//! @brief Frame ID of the robot's body frame
//!
//! This is needed for obtaining transforms from the robot's body frame to the frames of sensors (IMU and GPS)
//!
std::string base_link_frame_id_;
//! @brief The cartesian frame ID, default as 'local_enu' if using Local Cartesian or 'utm' if using the UTM
//! coordinates as our cartesian.
//!
std::string cartesian_frame_id_;
//! @brief The frame_id of the GPS message (specifies mounting location)
//!
std::string gps_frame_id_;
//! @brief the UTM zone (zero means UPS)
//!
int utm_zone_;
//! @brief hemisphere (true means north, false means south)
//!
bool northp_;
//! @brief Frame ID of the GPS odometry output
//!
//! This will just match whatever your odometry message has
//!
std::string world_frame_id_;
//! @brief Covariance for most recent odometry data
//!
Eigen::MatrixXd latest_odom_covariance_;
//! @brief Covariance for most recent GPS/UTM/LocalCartesian data
//!
Eigen::MatrixXd latest_cartesian_covariance_;
//! @brief Timestamp of the latest good GPS message
//!
//! We assign this value to the timestamp of the odometry message that we output
//!
ros::Time gps_update_time_;
//! @brief Timestamp of the latest good odometry message
//!
//! We assign this value to the timestamp of the odometry message that we output
//!
ros::Time odom_update_time_;
//! @brief Parameter that specifies the how long we wait for a transform to become available.
//!
ros::Duration transform_timeout_;
//! @brief timer calling periodicUpdate
//!
ros::Timer periodicUpdateTimer_;
//! @brief Latest IMU orientation
//!
tf2::Quaternion transform_orientation_;
//! @brief Latest GPS data, stored as Cartesian coords
//!
tf2::Transform latest_cartesian_pose_;
//! @brief Latest odometry pose data
//!
tf2::Transform latest_world_pose_;
//! @brief Holds the cartesian (UTM or local ENU) pose that is used to compute the transform
//!
tf2::Transform transform_cartesian_pose_;
//! @brief Latest IMU orientation
//!
tf2::Transform transform_world_pose_;
//! @brief Holds the Cartesian->odom transform
//!
tf2::Transform cartesian_world_transform_;
//! @brief Holds the odom->UTM transform for filtered GPS broadcast
//!
tf2::Transform cartesian_world_trans_inverse_;
//! @brief Publiser for filtered gps data
//!
ros::Publisher filtered_gps_pub_;
//! @brief GPS subscriber
//!
ros::Subscriber gps_sub_;
//! @brief Subscribes to imu topic
//!
ros::Subscriber imu_sub_;
//! @brief Odometry subscriber
//!
ros::Subscriber odom_sub_;
//! @brief Publisher for gps data
//!
ros::Publisher gps_odom_pub_;
//! @brief Service for set datum
//!
ros::ServiceServer datum_srv_;
//! @brief Service for to Lat Long
//!
ros::ServiceServer to_ll_srv_;
//! @brief Service for from Lat Long
//!
ros::ServiceServer from_ll_srv_;
//! @brief Service for set UTM zone
//!
ros::ServiceServer set_utm_zone_srv_;
//! @brief Transform buffer for managing coordinate transforms
//!
tf2_ros::Buffer tf_buffer_;
//! @brief Transform listener for receiving transforms
//!
tf2_ros::TransformListener tf_listener_;
//! @brief Used for publishing the static world_frame->cartesian transform
//!
tf2_ros::StaticTransformBroadcaster cartesian_broadcaster_;
};
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H

222
Localizations/Packages/robot_localization/include/robot_localization/robot_localization_estimator.h Normal file
View File

@@ -0,0 +1,222 @@
/*
* Copyright (c) 2016, TNO IVS Helmond.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H
#define ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H
#include <iostream>
#include <vector>
#include <boost/circular_buffer.hpp>
#include <Eigen/Dense>
#include "robot_localization/filter_base.h"
#include "robot_localization/filter_utilities.h"
#include "robot_localization/filter_common.h"
namespace RobotLocalization
{
struct Twist
{
Eigen::Vector3d linear;
Eigen::Vector3d angular;
};
//! @brief Robot Localization Estimator State
//!
//! The Estimator State data structure bundles the state information of the estimator.
//!
struct EstimatorState
{
EstimatorState():
time_stamp(0.0),
state(STATE_SIZE),
covariance(STATE_SIZE, STATE_SIZE)
{
state.setZero();
covariance.setZero();
}
//! @brief Time at which this state is/was achieved
double time_stamp;
//! @brief System state at time = time_stamp
Eigen::VectorXd state;
//! @brief System state covariance at time = time_stamp
Eigen::MatrixXd covariance;
friend std::ostream& operator<<(std::ostream &os, const EstimatorState& state)
{
return os << "state:\n - time_stamp: " << state.time_stamp <<
"\n - state: \n" << state.state <<
" - covariance: \n" << state.covariance;
}
};
namespace EstimatorResults
{
enum EstimatorResult
{
ExtrapolationIntoFuture = 0,
Interpolation,
ExtrapolationIntoPast,
Exact,
EmptyBuffer,
Failed
};
} // namespace EstimatorResults
typedef EstimatorResults::EstimatorResult EstimatorResult;
namespace FilterTypes
{
enum FilterType
{
EKF = 0,
UKF,
NotDefined
};
} // namespace FilterTypes
typedef FilterTypes::FilterType FilterType;
//! @brief Robot Localization Listener class
//!
//! The Robot Localization Estimator class buffers states of and inputs to a system and can interpolate and extrapolate
//! based on a given system model.
//!
class RobotLocalizationEstimator
{
public:
//! @brief Constructor for the RobotLocalizationListener class
//!
//! @param[in] args - Generic argument container (not used here, but needed so that the ROS filters can pass arbitrary
//! arguments to templated filter types).
//!
explicit RobotLocalizationEstimator(unsigned int buffer_capacity,
FilterType filter_type,
const Eigen::MatrixXd& process_noise_covariance,
const std::vector<double>& filter_args = std::vector<double>());
//! @brief Destructor for the RobotLocalizationListener class
//!
virtual ~RobotLocalizationEstimator();
//! @brief Sets the current internal state of the listener.
//!
//! @param[in] state - The new state vector to set the internal state to
//!
void setState(const EstimatorState& state);
//! @brief Returns the state at a given time
//!
//! Projects the current state and error matrices forward using a model of the robot's motion.
//!
//! @param[in] time - The time to which the prediction is being made
//! @param[out] state - The returned state at the given time
//!
//! @return GetStateResult enum
//!
EstimatorResult getState(const double time, EstimatorState &state) const;
//! @brief Clears the internal state buffer
//!
void clearBuffer();
//! @brief Sets the buffer capacity
//!
//! @param[in] capacity - The new buffer capacity
//!
void setBufferCapacity(const int capacity);
//! @brief Returns the buffer capacity
//!
//! Returns the number of EstimatorState objects that can be pushed to the buffer before old ones are dropped. (The
//! capacity of the buffer).
//!
//! @return buffer capacity
//!
unsigned int getBufferCapacity() const;
//! @brief Returns the current buffer size
//!
//! Returns the number of EstimatorState objects currently in the buffer.
//!
//! @return current buffer size
//!
unsigned int getSize() const;
private:
friend std::ostream& operator<<(std::ostream &os, const RobotLocalizationEstimator& rle)
{
for ( boost::circular_buffer<EstimatorState>::const_iterator it = rle.state_buffer_.begin();
it != rle.state_buffer_.end(); ++it )
{
os << *it << "\n";
}
return os;
}
//! @brief Extrapolates the given state to a requested time stamp
//!
//! @param[in] boundary_state - state from which to extrapolate
//! @param[in] requested_time - time stamp to extrapolate to
//! @param[out] state_at_req_time - predicted state at requested time
//!
void extrapolate(const EstimatorState& boundary_state,
const double requested_time,
EstimatorState& state_at_req_time) const;
//! @brief Interpolates the given state to a requested time stamp
//!
//! @param[in] given_state_1 - last state update before requested time
//! @param[in] given_state_2 - next state update after requested time
//! @param[in] requested_time - time stamp to extrapolate to
//! @param[out] state_at_req_time - predicted state at requested time
//!
void interpolate(const EstimatorState& given_state_1, const EstimatorState& given_state_2,
const double requested_time, EstimatorState& state_at_req_time) const;
//!
//! @brief The buffer holding the system states that have come in. Interpolation and extrapolation is done starting
//! from these states.
//!
boost::circular_buffer<EstimatorState> state_buffer_;
//!
//! @brief A pointer to the filter instance that is used for extrapolation
//!
FilterBase* filter_;
};
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H

767
Localizations/Packages/robot_localization/include/robot_localization/ros_filter.h Normal file
View File

@@ -0,0 +1,767 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_ROS_FILTER_H
#define ROBOT_LOCALIZATION_ROS_FILTER_H
#include "robot_localization/ros_filter_utilities.h"
#include "robot_localization/filter_common.h"
#include "robot_localization/filter_base.h"
#include <robot_localization/SetPose.h>
#include <robot_localization/ToggleFilterProcessing.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/AccelWithCovarianceStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/message_filter.h>
#include <tf2/LinearMath/Transform.h>
#include <message_filters/subscriber.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <XmlRpcException.h>
#include <Eigen/Dense>
#include <fstream>
#include <map>
#include <memory>
#include <numeric>
#include <queue>
#include <string>
#include <vector>
#include <deque>
namespace RobotLocalization
{
struct CallbackData
{
CallbackData(const std::string &topicName,
const std::vector<int> &updateVector,
const int updateSum,
const bool differential,
const bool relative,
const bool pose_use_child_frame,
const double rejectionThreshold) :
topicName_(topicName),
updateVector_(updateVector),
updateSum_(updateSum),
differential_(differential),
relative_(relative),
pose_use_child_frame_(pose_use_child_frame),
rejectionThreshold_(rejectionThreshold)
{
}
std::string topicName_;
std::vector<int> updateVector_;
int updateSum_;
bool differential_;
bool relative_;
bool pose_use_child_frame_;
double rejectionThreshold_;
};
typedef std::priority_queue<MeasurementPtr, std::vector<MeasurementPtr>, Measurement> MeasurementQueue;
typedef std::deque<MeasurementPtr> MeasurementHistoryDeque;
typedef std::deque<FilterStatePtr> FilterStateHistoryDeque;
template<class T> class RosFilter
{
public:
//! @brief Constructor
//!
//! The RosFilter constructor makes sure that anyone using
//! this template is doing so with the correct object type
//!
explicit RosFilter(ros::NodeHandle nh,
ros::NodeHandle nh_priv,
std::string node_name,
std::vector<double> args = std::vector<double>());
//! @brief Constructor
//!
//! The RosFilter constructor makes sure that anyone using
//! this template is doing so with the correct object type
//!
explicit RosFilter(ros::NodeHandle nh, ros::NodeHandle nh_priv, std::vector<double> args = std::vector<double>());
//! @brief Destructor
//!
//! Clears out the message filters and topic subscribers.
//!
~RosFilter();
//! @brief Initialize filter
//
void initialize();
//! @brief Resets the filter to its initial state
//!
void reset();
//! @brief Service callback to toggle processing measurements for a standby mode but continuing to publish
//! @param[in] request - The state requested, on (True) or off (False)
//! @param[out] response - status if upon success
//! @return boolean true if successful, false if not
bool toggleFilterProcessingCallback(robot_localization::ToggleFilterProcessing::Request&,
robot_localization::ToggleFilterProcessing::Response&);
//! @brief Callback method for receiving all acceleration (IMU) messages
//! @param[in] msg - The ROS IMU message to take in.
//! @param[in] callbackData - Relevant static callback data
//! @param[in] targetFrame - The target frame_id into which to transform the data
//!
void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,
const CallbackData &callbackData,
const std::string &targetFrame);
//! @brief Callback method for receiving non-stamped control input
//! @param[in] msg - The ROS twist message to take in
//!
void controlCallback(const geometry_msgs::Twist::ConstPtr &msg);
//! @brief Callback method for receiving stamped control input
//! @param[in] msg - The ROS stamped twist message to take in
//!
void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);
//! @brief Adds a measurement to the queue of measurements to be processed
//!
//! @param[in] topicName - The name of the measurement source (only used for debugging)
//! @param[in] measurement - The measurement to enqueue
//! @param[in] measurementCovariance - The covariance of the measurement
//! @param[in] updateVector - The boolean vector that specifies which variables to update from this measurement
//! @param[in] mahalanobisThresh - Threshold, expressed as a Mahalanobis distance, for outlier rejection
//! @param[in] time - The time of arrival (in seconds)
//!
void enqueueMeasurement(const std::string &topicName,
const Eigen::VectorXd &measurement,
const Eigen::MatrixXd &measurementCovariance,
const std::vector<int> &updateVector,
const double mahalanobisThresh,
const ros::Time &time);
//! @brief Method for zeroing out 3D variables within measurements
//! @param[out] measurement - The measurement whose 3D variables will be zeroed out
//! @param[out] measurementCovariance - The covariance of the measurement
//! @param[out] updateVector - The boolean update vector of the measurement
//!
//! If we're in 2D mode, then for every measurement from every sensor, we call this.
//! It sets the 3D variables to 0, gives those variables tiny variances, and sets
//! their updateVector values to 1.
//!
void forceTwoD(Eigen::VectorXd &measurement,
Eigen::MatrixXd &measurementCovariance,
std::vector<int> &updateVector);
//! @brief Retrieves the EKF's output for broadcasting
//! @param[out] message - The standard ROS odometry message to be filled
//! @return true if the filter is initialized, false otherwise
//!
bool getFilteredOdometryMessage(nav_msgs::Odometry &message);
//! @brief Retrieves the EKF's acceleration output for broadcasting
//! @param[out] message - The standard ROS acceleration message to be filled
//! @return true if the filter is initialized, false otherwise
//!
bool getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message);
//! @brief Callback method for receiving all IMU messages
//! @param[in] msg - The ROS IMU message to take in.
//! @param[in] topicName - The topic name for the IMU message (only used for debug output)
//! @param[in] poseCallbackData - Relevant static callback data for orientation variables
//! @param[in] twistCallbackData - Relevant static callback data for angular velocity variables
//! @param[in] accelCallbackData - Relevant static callback data for linear acceleration variables
//!
//! This method separates out the orientation, angular velocity, and linear acceleration data and
//! passed each on to its respective callback.
//!
void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName,
const CallbackData &poseCallbackData, const CallbackData &twistCallbackData,
const CallbackData &accelCallbackData);
//! @brief Processes all measurements in the measurement queue, in temporal order
//!
//! @param[in] currentTime - The time at which to carry out integration (the current time)
//!
void integrateMeasurements(const ros::Time &currentTime);
//! @brief Differentiate angular velocity for angular acceleration
//!
//! @param[in] currentTime - The time at which to carry out differentiation (the current time)
//!
//! Maybe more state variables can be time-differentiated to estimate higher-order states,
//! but now we only focus on obtaining the angular acceleration. It implements a backward-
//! Euler differentiation.
//!
void differentiateMeasurements(const ros::Time &currentTime);
//! @brief Loads all parameters from file
//!
void loadParams();
//! @brief Callback method for receiving all odometry messages
//! @param[in] msg - The ROS odometry message to take in.
//! @param[in] topicName - The topic name for the odometry message (only used for debug output)
//! @param[in] poseCallbackData - Relevant static callback data for pose variables
//! @param[in] twistCallbackData - Relevant static callback data for twist variables
//!
//! This method simply separates out the pose and twist data into two new messages, and passes them into their
//! respective callbacks
//!
void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,
const CallbackData &poseCallbackData, const CallbackData &twistCallbackData);
//! @brief Callback method for receiving all pose messages
//! @param[in] msg - The ROS stamped pose with covariance message to take in
//! @param[in] callbackData - Relevant static callback data
//! @param[in] targetFrame - The target frame_id into which to transform the data
//! @param[in] poseSourceFrame - The source frame_id from which to transform the data
//! @param[in] imuData - Whether this data comes from an IMU
//!
void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
const CallbackData &callbackData,
const std::string &targetFrame,
const std::string &poseSourceFrame,
const bool imuData);
//! @brief Callback method for manually setting/resetting the internal pose estimate
//! @param[in] msg - The ROS stamped pose with covariance message to take in
//!
void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
//! @brief Service callback for manually setting/resetting the internal pose estimate
//!
//! @param[in] request - Custom service request with pose information
//! @return true if successful, false if not
bool setPoseSrvCallback(robot_localization::SetPose::Request& request,
robot_localization::SetPose::Response&);
//! @brief Service callback for manually enable the filter
//! @param[in] request - N/A
//! @param[out] response - N/A
//! @return boolean true if successful, false if not
bool enableFilterSrvCallback(std_srvs::Empty::Request&,
std_srvs::Empty::Response&);
//! @brief Callback method for receiving all twist messages
//! @param[in] msg - The ROS stamped twist with covariance message to take in.
//! @param[in] callbackData - Relevant static callback data
//! @param[in] targetFrame - The target frame_id into which to transform the data
//!
void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
const CallbackData &callbackData,
const std::string &targetFrame);
//! @brief Validates filter outputs for NaNs and Infinite values
//! @param[out] message - The standard ROS odometry message to be validated
//! @return true if the filter output is valid, false otherwise
//!
bool validateFilterOutput(const nav_msgs::Odometry &message);
protected:
//! @brief Finds the latest filter state before the given timestamp and makes it the current state again.
//!
//! This method also inserts all measurements between the older filter timestamp and now into the measurements
//! queue.
//!
//! @param[in] time - The time to which the filter state should revert
//! @return True if restoring the filter succeeded. False if not.
//!
bool revertTo(const double time);
//! @brief Saves the current filter state in the queue of previous filter states
//!
//! These measurements will be used in backwards smoothing in the event that older measurements come in.
//! @param[in] filter - The filter base object whose state we want to save
//!
void saveFilterState(FilterBase &filter);
//! @brief Removes measurements and filter states older than the given cutoff time.
//! @param[in] cutoffTime - Measurements and states older than this time will be dropped.
//!
void clearExpiredHistory(const double cutoffTime);
//! @brief Clears measurement queue
//!
void clearMeasurementQueue();
//! @brief Adds a diagnostic message to the accumulating map and updates the error level
//! @param[in] errLevel - The error level of the diagnostic
//! @param[in] topicAndClass - The sensor topic (if relevant) and class of diagnostic
//! @param[in] message - Details of the diagnostic
//! @param[in] staticDiag - Whether or not this diagnostic information is static
//!
void addDiagnostic(const int errLevel,
const std::string &topicAndClass,
const std::string &message,
const bool staticDiag);
//! @brief Aggregates all diagnostics so they can be published
//! @param[in] wrapper - The diagnostic status wrapper to update
//!
void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper);
//! @brief Utility method for copying covariances from ROS covariance arrays
//! to Eigen matrices
//!
//! This method copies the covariances and also does some data validation, which is
//! why it requires more parameters than just the covariance containers.
//! @param[in] arr - The source array for the covariance data
//! @param[in] covariance - The destination matrix for the covariance data
//! @param[in] topicName - The name of the source data topic (for debug purposes)
//! @param[in] updateVector - The update vector for the source topic
//! @param[in] offset - The "starting" location within the array/update vector
//! @param[in] dimension - The number of values to copy, starting at the offset
//!
void copyCovariance(const double *arr,
Eigen::MatrixXd &covariance,
const std::string &topicName,
const std::vector<int> &updateVector,
const size_t offset,
const size_t dimension);
//! @brief Utility method for copying covariances from Eigen matrices to ROS
//! covariance arrays
//!
//! @param[in] covariance - The source matrix for the covariance data
//! @param[in] arr - The destination array
//! @param[in] dimension - The number of values to copy
//!
void copyCovariance(const Eigen::MatrixXd &covariance,
double *arr,
const size_t dimension);
//! @brief Loads fusion settings from the config file
//! @param[in] topicName - The name of the topic for which to load settings
//! @return The boolean vector of update settings for each variable for this topic
//!
std::vector<int> loadUpdateConfig(const std::string &topicName);
//! @brief Prepares an IMU message's linear acceleration for integration into the filter
//! @param[in] msg - The IMU message to prepare
//! @param[in] topicName - The name of the topic over which this message was received
//! @param[in] targetFrame - The target tf frame
//! @param[in] updateVector - The update vector for the data source
//! @param[in] measurement - The twist data converted to a measurement
//! @param[in] measurementCovariance - The covariance of the converted measurement
//!
bool prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg,
const std::string &topicName,
const std::string &targetFrame,
const bool relative,
std::vector<int> &updateVector,
Eigen::VectorXd &measurement,
Eigen::MatrixXd &measurementCovariance);
//! @brief Prepares a pose message for integration into the filter
//! @param[in] msg - The pose message to prepare
//! @param[in] topicName - The name of the topic over which this message was received
//! @param[in] targetFrame - The target tf frame
//! @param[in] sourceFrame - The source tf frame
//! @param[in] differential - Whether we're carrying out differential integration
//! @param[in] relative - Whether this measurement is processed relative to the first
//! @param[in] imuData - Whether this measurement is from an IMU
//! @param[in,out] updateVector - The update vector for the data source
//! @param[out] measurement - The pose data converted to a measurement
//! @param[out] measurementCovariance - The covariance of the converted measurement
//! @return true indicates that the measurement was successfully prepared, false otherwise
//!
bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
const std::string &topicName,
const std::string &targetFrame,
const std::string &sourceFrame,
const bool differential,
const bool relative,
const bool imuData,
std::vector<int> &updateVector,
Eigen::VectorXd &measurement,
Eigen::MatrixXd &measurementCovariance);
//! @brief Prepares a twist message for integration into the filter
//! @param[in] msg - The twist message to prepare
//! @param[in] topicName - The name of the topic over which this message was received
//! @param[in] targetFrame - The target tf frame
//! @param[in,out] updateVector - The update vector for the data source
//! @param[out] measurement - The twist data converted to a measurement
//! @param[out] measurementCovariance - The covariance of the converted measurement
//! @return true indicates that the measurement was successfully prepared, false otherwise
//!
bool prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
const std::string &topicName,
const std::string &targetFrame,
std::vector<int> &updateVector,
Eigen::VectorXd &measurement,
Eigen::MatrixXd &measurementCovariance);
//! @brief callback function which is called for periodic updates
//!
void periodicUpdate(const ros::TimerEvent& event);
//! @brief Start the Filter disabled at startup
//!
//! If this is true, the filter reads parameters and prepares publishers and subscribes
//! but does not integrate new messages into the state vector.
//! The filter can be enabled later using a service.
bool disabledAtStartup_;
//! @brief Whether the filter is enabled or not. See disabledAtStartup_.
bool enabled_;
//! Whether we'll allow old measurements to cause a re-publication of the updated state
bool permitCorrectedPublication_;
//! @brief By default, the filter predicts and corrects up to the time of the latest measurement. If this is set
//! to true, the filter does the same, but then also predicts up to the current time step.
//!
bool predictToCurrentTime_;
//! @brief Whether or not we print diagnostic messages to the /diagnostics topic
//!
bool printDiagnostics_;
//! @brief Whether we publish the acceleration
//!
bool publishAcceleration_;
//! @brief Whether we publish the transform from the world_frame to the base_link_frame
//!
bool publishTransform_;
//! @brief Whether to reset the filters when backwards jump in time is detected
//!
//! This is usually the case when logs are being used and a jump in the logi
//! is done or if a log files restarts from the beginning.
//!
bool resetOnTimeJump_;
//! @brief Whether or not we use smoothing
//!
bool smoothLaggedData_;
//! @brief Whether the filter should process new measurements or not.
bool toggledOn_;
//! @brief Whether or not we're in 2D mode
//!
//! If this is true, the filter binds all 3D variables (Z,
//! roll, pitch, and their respective velocities) to 0 for
//! every measurement.
//!
bool twoDMode_;
//! @brief Whether or not we use a control term
//!
bool useControl_;
//! @brief When true, do not print warnings for tf lookup failures.
//!
bool tfSilentFailure_;
//! @brief The max (worst) dynamic diagnostic level.
//!
int dynamicDiagErrorLevel_;
//! @brief The max (worst) static diagnostic level.
//!
int staticDiagErrorLevel_;
//! @brief The frequency of the run loop
//!
double frequency_;
//! @brief What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665.
//!
double gravitationalAcc_;
//! @brief The depth of the history we track for smoothing/delayed measurement processing
//!
//! This is the guaranteed minimum buffer size for which previous states and measurements are kept.
//!
double historyLength_;
//! @brief minimal frequency
//!
double minFrequency_;
//! @brief maximal frequency
double maxFrequency_;
//! @brief tf frame name for the robot's body frame
//!
std::string baseLinkFrameId_;
//! @brief tf frame name for the robot's body frame
//!
//! When the final state is computed, we "override" the output transform and message to have this frame for its
//! child_frame_id. This helps to enable disconnected TF trees when multiple EKF instances are being run.
//!
std::string baseLinkOutputFrameId_;
//! @brief tf frame name for the robot's map (world-fixed) frame
//!
std::string mapFrameId_;
//! @brief tf frame name for the robot's odometry (world-fixed) frame
//!
std::string odomFrameId_;
//! @brief tf frame name that is the parent frame of the transform that this node will calculate and broadcast.
//!
std::string worldFrameId_;
//! @brief Used for outputting debug messages
//!
std::ofstream debugStream_;
//! @brief Contains the state vector variable names in string format
//!
std::vector<std::string> stateVariableNames_;
//! @brief Vector to hold our subscribers until they go out of scope
//!
std::vector<ros::Subscriber> topicSubs_;
//! @brief This object accumulates dynamic diagnostics, e.g., diagnostics relating
//! to sensor data.
//!
//! The values are considered transient and are cleared at every iteration.
//!
std::map<std::string, std::string> dynamicDiagnostics_;
//! @brief Stores the first measurement from each topic for relative measurements
//!
//! When a given sensor is being integrated in relative mode, its first measurement
//! is effectively treated as an offset, and future measurements have this first
//! measurement removed before they are fused. This variable stores the initial
//! measurements. Note that this is different from using differential mode, as in
//! differential mode, pose data is converted to twist data, resulting in boundless
//! error growth for the variables being fused. With relative measurements, the
//! vehicle will start with a 0 heading and position, but the measurements are still
//! fused absolutely.
std::map<std::string, tf2::Transform> initialMeasurements_;
//! @brief Store the last time a message from each topic was received
//!
//! If we're getting messages rapidly, we may accidentally get
//! an older message arriving after a newer one. This variable keeps
//! track of the most recent message time for each subscribed message
//! topic. We also use it when listening to odometry messages to
//! determine if we should be using messages from that topic.
//!
std::map<std::string, ros::Time> lastMessageTimes_;
//! @brief We also need the previous covariance matrix for differential data
//!
std::map<std::string, Eigen::MatrixXd> previousMeasurementCovariances_;
//! @brief Stores the last measurement from a given topic for differential integration
//!
//! To carry out differential integration, we have to (1) transform
//! that into the target frame (probably the frame specified by
//! @p odomFrameId_), (2) "subtract" the previous measurement from
//! the current measurement, and then (3) transform it again by the previous
//! state estimate. This holds the measurements used for step (2).
//!
std::map<std::string, tf2::Transform> previousMeasurements_;
//! @brief If including acceleration for each IMU input, whether or not we remove acceleration due to gravity
//!
std::map<std::string, bool> removeGravitationalAcc_;
//! @brief This object accumulates static diagnostics, e.g., diagnostics relating
//! to the configuration parameters.
//!
//! The values are treated as static and always reported (i.e., this object is never cleared)
//!
std::map<std::string, std::string> staticDiagnostics_;
//! @brief Last time mark that time-differentiation is calculated
//!
ros::Time lastDiffTime_;
//! @brief Last record of filtered angular velocity
//!
tf2::Vector3 lastStateTwistRot_;
//! @brief Calculated angular acceleration from time-differencing
//!
tf2::Vector3 angular_acceleration_;
//! @brief Covariance of the calculated angular acceleration
//!
Eigen::MatrixXd angular_acceleration_cov_;
//! @brief The most recent control input
//!
Eigen::VectorXd latestControl_;
//! @brief Message that contains our latest transform (i.e., state)
//!
//! We use the vehicle's latest state in a number of places, and often
//! use it as a transform, so this is the most convenient variable to
//! use as our global "current state" object
//!
geometry_msgs::TransformStamped worldBaseLinkTransMsg_;
//! @brief last call of periodicUpdate
//!
ros::Time lastDiagTime_;
//! @brief The time of the most recent published state
//!
ros::Time lastPublishedStamp_;
//! @brief Store the last time set pose message was received
//!
//! If we receive a setPose message to reset the filter, we can get in
//! strange situations wherein we process the pose reset, but then even
//! after another spin cycle or two, we can get a new message with a time
//! stamp that is *older* than the setPose message's time stamp. This means
//! we have to check the message's time stamp against the lastSetPoseTime_.
ros::Time lastSetPoseTime_;
//! @brief The time of the most recent control input
//!
ros::Time latestControlTime_;
//! @brief For future (or past) dating the world_frame->base_link_frame transform
//!
ros::Duration tfTimeOffset_;
//! @brief Parameter that specifies the how long we wait for a transform to become available.
//!
ros::Duration tfTimeout_;
//! @brief Service that allows another node to toggle on/off filter processing while still publishing.
//! Uses a robot_localization ToggleFilterProcessing service.
//!
ros::ServiceServer toggleFilterProcessingSrv_;
//! @brief timer calling periodicUpdate
//!
ros::Timer periodicUpdateTimer_;
//! @brief An implicitly time ordered queue of past filter states used for smoothing.
//
// front() refers to the filter state with the earliest timestamp.
// back() refers to the filter state with the latest timestamp.
FilterStateHistoryDeque filterStateHistory_;
//! @brief A deque of previous measurements which is implicitly ordered from earliest to latest measurement.
// when popped from the measurement priority queue.
// front() refers to the measurement with the earliest timestamp.
// back() refers to the measurement with the latest timestamp.
MeasurementHistoryDeque measurementHistory_;
//! @brief We process measurements by queueing them up in
//! callbacks and processing them all at once within each
//! iteration
//!
MeasurementQueue measurementQueue_;
//! @brief Our filter (EKF, UKF, etc.)
//!
T filter_;
//! @brief Node handle
//!
ros::NodeHandle nh_;
//! @brief Local node handle (for params)
//!
ros::NodeHandle nhLocal_;
//! @brief optional acceleration publisher
//!
ros::Publisher accelPub_;
//! @brief position publisher
//!
ros::Publisher positionPub_;
//! @brief Subscribes to the control input topic
//!
ros::Subscriber controlSub_;
//! @brief Subscribes to the set_pose topic (usually published from rviz). Message
//! type is geometry_msgs/PoseWithCovarianceStamped.
//!
ros::Subscriber setPoseSub_;
//! @brief Service that allows another node to enable the filter. Uses a standard Empty service.
//!
ros::ServiceServer enableFilterSrv_;
//! @brief Service that allows another node to change the current state and recieve a confirmation. Uses
//! a custom SetPose service.
//!
ros::ServiceServer setPoseSrv_;
//! @brief Used for updating the diagnostics
//!
diagnostic_updater::Updater diagnosticUpdater_;
//! @brief Transform buffer for managing coordinate transforms
//!
tf2_ros::Buffer tfBuffer_;
//! @brief Transform listener for receiving transforms
//!
tf2_ros::TransformListener tfListener_;
//! @brief broadcaster of worldTransform tfs
//!
tf2_ros::TransformBroadcaster worldTransformBroadcaster_;
//! @brief optional signaling diagnostic frequency
//!
std::unique_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> freqDiag_;
};
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_ROS_FILTER_H

48
Localizations/Packages/robot_localization/include/robot_localization/ros_filter_types.h Normal file
View File

@@ -0,0 +1,48 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H
#define ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H
#include "robot_localization/ros_filter.h"
#include "robot_localization/ekf.h"
#include "robot_localization/ukf.h"
namespace RobotLocalization
{
typedef RosFilter<Ekf> RosEkf;
typedef RosFilter<Ukf> RosUkf;
}
#endif // ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H

135
Localizations/Packages/robot_localization/include/robot_localization/ros_filter_utilities.h Normal file
View File

@@ -0,0 +1,135 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H
#define ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#define RF_DEBUG(msg) if (filter_.getDebug()) { debugStream_ << msg; }
// Handy methods for debug output
std::ostream& operator<<(std::ostream& os, const tf2::Vector3 &vec);
std::ostream& operator<<(std::ostream& os, const tf2::Quaternion &quat);
std::ostream& operator<<(std::ostream& os, const tf2::Transform &trans);
std::ostream& operator<<(std::ostream& os, const std::vector<double> &vec);
namespace RobotLocalization
{
namespace RosFilterUtilities
{
double getYaw(const tf2::Quaternion quat);
//! @brief Method for safely obtaining transforms.
//! @param[in] buffer - tf buffer object to use for looking up the transform
//! @param[in] targetFrame - The target frame of the desired transform
//! @param[in] sourceFrame - The source frame of the desired transform
//! @param[in] time - The time at which we want the transform
//! @param[in] timeout - How long to block before falling back to last transform
//! @param[out] targetFrameTrans - The resulting transform object
//! @param[in] silent - Whether or not to print transform warnings
//! @return Sets the value of @p targetFrameTrans and returns true if successful,
//! false otherwise.
//!
//! This method attempts to obtain a transform from the @p sourceFrame to the @p
//! targetFrame at the specific @p time. If no transform is available at that time,
//! it attempts to simply obtain the latest transform. If that still fails, then the
//! method checks to see if the transform is going from a given frame_id to itself.
//! If any of these checks succeed, the method sets the value of @p targetFrameTrans
//! and returns true, otherwise it returns false.
//!
bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
const std::string &targetFrame,
const std::string &sourceFrame,
const ros::Time &time,
const ros::Duration &timeout,
tf2::Transform &targetFrameTrans,
const bool silent = false);
//! @brief Method for safely obtaining transforms.
//! @param[in] buffer - tf buffer object to use for looking up the transform
//! @param[in] targetFrame - The target frame of the desired transform
//! @param[in] sourceFrame - The source frame of the desired transform
//! @param[in] time - The time at which we want the transform
//! @param[out] targetFrameTrans - The resulting transform object
//! @param[in] silent - Whether or not to print transform warnings
//! @return Sets the value of @p targetFrameTrans and returns true if successful,
//! false otherwise.
//!
//! This method attempts to obtain a transform from the @p sourceFrame to the @p
//! targetFrame at the specific @p time. If no transform is available at that time,
//! it attempts to simply obtain the latest transform. If that still fails, then the
//! method checks to see if the transform is going from a given frame_id to itself.
//! If any of these checks succeed, the method sets the value of @p targetFrameTrans
//! and returns true, otherwise it returns false.
//!
bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
const std::string &targetFrame,
const std::string &sourceFrame,
const ros::Time &time,
tf2::Transform &targetFrameTrans,
const bool silent = false);
//! @brief Utility method for converting quaternion to RPY
//! @param[in] quat - The quaternion to convert
//! @param[out] roll - The converted roll
//! @param[out] pitch - The converted pitch
//! @param[out] yaw - The converted yaw
//!
void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw);
//! @brief Converts our Eigen state vector into a TF transform/pose
//! @param[in] state - The state to convert
//! @param[out] stateTF - The converted state
//!
void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF);
//! @brief Converts a TF transform/pose into our Eigen state vector
//! @param[in] stateTF - The state to convert
//! @param[out] state - The converted state
//!
void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state);
} // namespace RosFilterUtilities
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H

171
Localizations/Packages/robot_localization/include/robot_localization/ros_robot_localization_listener.h Normal file
View File

@@ -0,0 +1,171 @@
/*
* Copyright (c) 2016, TNO IVS Helmond.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H
#define ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H
#include "robot_localization/robot_localization_estimator.h"
#include <string>
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/AccelWithCovarianceStamped.h>
#include <tf2_ros/transform_listener.h>
namespace RobotLocalization
{
//! @brief RosRobotLocalizationListener class
//!
//! This class wraps the RobotLocalizationEstimator. It listens to topics over which the (filtered) robot state is
//! published (odom and accel) and pushes them into its instance of the RobotLocalizationEstimator. It exposes a
//! getState method to offer the user the estimated state at a requested time. This class offers the option to run this
//! listener without the need to run a separate node. If you do wish to run this functionality in a separate node,
//! consider the robot localization listener node.
//!
class RosRobotLocalizationListener
{
public:
//! @brief Constructor
//!
//! The RosRobotLocalizationListener constructor initializes nodehandles, subscribers, a filter for synchronized
//! listening to the topics it subscribes to, and an instance of the RobotLocalizationEstimator.
//!
RosRobotLocalizationListener();
//! @brief Destructor
//!
//! Empty destructor
//!
~RosRobotLocalizationListener();
//! @brief Get a state from the localization estimator
//!
//! Requests the predicted state and covariance at a given time from the Robot Localization Estimator.
//!
//! @param[in] time - time of the requested state
//! @param[in] frame_id - frame id of which the state is requested.
//! @param[out] state - state at the requested time
//! @param[out] covariance - covariance at the requested time
//!
//! @return false if buffer is empty, true otherwise
//!
bool getState(const double time, const std::string& frame_id,
Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
std::string world_frame_id = "") const;
//! @brief Get a state from the localization estimator
//!
//! Overload of getState method for using ros::Time.
//!
//! @param[in] ros_time - ros time of the requested state
//! @param[in] frame_id - frame id of which the state is requested.
//! @param[out] state - state at the requested time
//! @param[out] covariance - covariance at the requested time
//!
//! @return false if buffer is empty, true otherwise
//!
bool getState(const ros::Time& ros_time, const std::string& frame_id,
Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
const std::string& world_frame_id = "") const;
//!
//! \brief getBaseFrameId Returns the base frame id of the localization listener
//! \return The base frame id
//!
const std::string& getBaseFrameId() const;
//!
//! \brief getWorldFrameId Returns the world frame id of the localization listener
//! \return The world frame id
//!
const std::string& getWorldFrameId() const;
private:
//! @brief Callback for odom and accel
//!
//! Puts the information from the odom and accel messages in a Robot Localization Estimator state and sets the most
//! recent state of the estimator.
//!
//! @param[in] odometry message
//! @param[in] accel message
//!
void odomAndAccelCallback(const nav_msgs::Odometry& odom, const geometry_msgs::AccelWithCovarianceStamped& accel);
//! @brief The core state estimator that facilitates inter- and
//! extrapolation between buffered states.
//!
RobotLocalizationEstimator* estimator_;
//! @brief A public handle to the ROS node
//!
ros::NodeHandle nh_;
//! @brief A private handle to the ROS node
//!
ros::NodeHandle nh_p_;
//! @brief Subscriber to the odometry state topic (output of a filter node)
//!
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
//! @brief Subscriber to the acceleration state topic (output of a filter node)
//!
message_filters::Subscriber<geometry_msgs::AccelWithCovarianceStamped> accel_sub_;
//! @brief Waits for both an Odometry and an Accel message before calling a single callback function
//!
message_filters::TimeSynchronizer<nav_msgs::Odometry, geometry_msgs::AccelWithCovarianceStamped> sync_;
//! @brief Child frame id received from the Odometry message
//!
std::string base_frame_id_;
//! @brief Frame id received from the odometry message
//!
std::string world_frame_id_;
//! @brief Tf buffer for looking up transforms
//!
tf2_ros::Buffer tf_buffer_;
//! @brief Transform listener to fill the tf_buffer
//!
tf2_ros::TransformListener tf_listener_;
};
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H

139
Localizations/Packages/robot_localization/include/robot_localization/ukf.h Normal file
View File

@@ -0,0 +1,139 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_LOCALIZATION_UKF_H
#define ROBOT_LOCALIZATION_UKF_H
#include "robot_localization/filter_base.h"
#include <fstream>
#include <vector>
#include <set>
#include <queue>
namespace RobotLocalization
{
//! @brief Unscented Kalman filter class
//!
//! Implementation of an unscenter Kalman filter (UKF). This
//! class derives from FilterBase and overrides the predict()
//! and correct() methods in keeping with the discrete time
//! UKF algorithm. The algorithm was derived from the UKF
//! Wikipedia article at
//! (http://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter)
//! ...and this paper:
//! J. J. LaViola, Jr., “A comparison of unscented and extended Kalman
//! filtering for estimating quaternion motion,” in Proc. American Control
//! Conf., Denver, CO, June 4–6, 2003, pp. 2435–2440
//! Obtained here: http://www.cs.ucf.edu/~jjl/pubs/laviola_acc2003.pdf
//!
class Ukf: public FilterBase
{
public:
//! @brief Constructor for the Ukf class
//!
//! @param[in] args - Generic argument container. It is assumed
//! that args[0] constains the alpha parameter, args[1] contains
//! the kappa parameter, and args[2] contains the beta parameter.
//!
explicit Ukf(std::vector<double> args);
//! @brief Destructor for the Ukf class
//!
~Ukf();
//! @brief Carries out the correct step in the predict/update cycle.
//!
//! @param[in] measurement - The measurement to fuse with our estimate
//!
void correct(const Measurement &measurement);
//! @brief Carries out the predict step in the predict/update cycle.
//!
//! Projects the state and error matrices forward using a model of
//! the vehicle's motion.
//!
//! @param[in] referenceTime - The time at which the prediction is being made
//! @param[in] delta - The time step over which to predict.
//!
void predict(const double referenceTime, const double delta);
protected:
//! @brief Computes the weighted covariance and sigma points
//!
void generateSigmaPoints();
//! @brief Carries out the predict step for the posteriori state of a sigma
//! point.
//!
//! Projects the state and error matrices forward using a model of
//! the vehicle's motion.
//!
//! @param[in,out] sigmaPoint - The sigma point (state vector) to project
//! @param[in] delta - The time step over which to project
//!
void projectSigmaPoint(Eigen::VectorXd& sigmaPoint, double delta);
//! @brief The UKF sigma points
//!
//! Used to sample possible next states during prediction.
//!
std::vector<Eigen::VectorXd> sigmaPoints_;
//! @brief This matrix is used to generate the sigmaPoints_
//!
Eigen::MatrixXd weightedCovarSqrt_;
//! @brief The weights associated with each sigma point when generating
//! a new state
//!
std::vector<double> stateWeights_;
//! @brief The weights associated with each sigma point when calculating
//! a predicted estimateErrorCovariance_
//!
std::vector<double> covarWeights_;
//! @brief Used in weight generation for the sigma points
//!
double lambda_;
//! @brief Used to determine if we need to re-compute the sigma
//! points when carrying out multiple corrections
//!
bool uncorrected_;
};
} // namespace RobotLocalization
#endif // ROBOT_LOCALIZATION_UKF_H

27
Localizations/Packages/robot_localization/launch/dual_ekf_navsat_example.launch Normal file
View File

@@ -0,0 +1,27 @@
<!--
This launch file provides an example of how to work with GPS data using robot_localization. It runs three nodes:
(1) An EKF instance that fuses odometry and IMU data and outputs an odom-frame state estimate
(2) A second EKF instance that fuses the same data, but also fuses the transformed GPS data from (3)
(3) An instance of navsat_transform_node, which takes in GPS data and produces pose data that has been
transformed into your robot's world frame (here, map). The node produces a map-frame state estimate.
The first EKF instance produces the odom->base_link transform. The second EKF produces the map->odom transform,
but requires the odom->base_link transform from the first instance in order to do so. See the
params/dual_ekf_navsat_example.yaml file for parameter specification.
-->
<launch>
<rosparam command="load" file="$(find robot_localization)/params/dual_ekf_navsat_example.yaml" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
</launch>

38
Localizations/Packages/robot_localization/launch/ekf_nodelet_template.launch Normal file
View File

@@ -0,0 +1,38 @@
<!--
This launch file provides an example of how to use ekf as nodelet or as node in one launch file.
By providing arguments like "use_nodelets this launch file will start a nodelet instead of a node.
This is very usefull in experimental setup to allow easy switch between nodelets and node.
Also it allows you to specify the manager the nodelet should run in.
-->
<launch>
<arg name="use_nodelets" default="${optenv USE_NODELETS false)" />
<arg name="nodelet_manager" default="$optenv robot_localization_NODELET_MANAGER robot_localization_nodelet_manager)" />
<!-- Placeholder for output topic remapping
<remap from="odometry/filtered" to=""/>
<remap from="accel/filtered" to=""/>
-->
<node unless="$(arg use_nodelets)"
pkg="robot_localization"
name="ekf_se"
type="ekf_localization_node"
clear_params="true"
output="screen"
>
<rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
</node>
<node if="$(arg use_nodelets)"
pkg="nodelet"
type="nodelet"
name="ekf_se"
output="screen"
args="load RobotLocalization/EkfNodelet $(arg nodelet_manager)"
>
<rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
</node>
</launch>

11
Localizations/Packages/robot_localization/launch/ekf_template.launch Normal file
View File

@@ -0,0 +1,11 @@
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
<rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
<!-- Placeholder for output topic remapping
<remap from="odometry/filtered" to=""/>
<remap from="accel/filtered" to=""/>
-->
</node>
</launch>

51
Localizations/Packages/robot_localization/launch/navsat_transform_template.launch Normal file
View File

@@ -0,0 +1,51 @@
<!--
This node needs to know the values of three variables in order to function:
(1) A world-referenced heading (yaw). The node assumes an ENU standard for heading, with 0 facing east, though it
can support any heading.
(2) Odometry data that gives the robot's current pose in its own world coordinate frame (typically map or odom)
(3) A latitude/longitude/altitude.
These three items allow us to compute a transform from the global frame to your robot's local frame. There are
several means of providing them, though keep in mind that these modes are typically mutually exclusive.
(1) World-referenced yaw can be provided by:
(a) an IMU in a sensor_msgs/Imu message (topic is /imu/data/)
(b) the heading in the nav_msgs/Odometry message in (2) below can be used. To enable this behavior, set the
use_odometry_yaw parameter to true, and set the delay parameter to some small value (~3 seconds). Be
careful, though: this heading must still be globally referenced, so if your state estimation node always
starts with a 0 heading, you CAN NOT use this option.
(c) the "datum" service. See the template parameter file (params/navsat_transform_template.yaml).
(2) The odometry data, which needs to have a valid frame_id, can be provided by:
(a) a nav_msgs/Odometry message from your robot_localization state estimation node.
(b) the "datum" service (all odometry variables are assumed to be 0 in this case). See the template
parameter file.
(3) The latitude, longitude, and altitude can be provided by:
(a) a sensor_msgs/NavSatFix message
(b) the "datum" service. See the template parameter file.
(4) Alternatively, at any time, the user can send a robot_localization/SetDatum service message to the "datum"
service. This will manually set the latitude, longitude, altitude, and world-referenced heading, and will
assume an odometry message containing all zeros. This will effectively set the origin at the specified
lat/long, with the X-axis aligned with east. The user can set this datum via the "datum" service, or via the
launch file. If the wait_for_datum parameter is set to true, then the node will attempt to use the datum
parameter. If the parameter is not set, it will wait until it receives a service call.
The output of this node is an odometry message that contains the GPS data transformed into the robot's world
coordinate frame (i.e., the frame specified by input (2)'s frame_id), or the coordinate frame defined by the
message sent to the "datum" service. Optionally, the node can also produce a NavSatFix message corresponding
to the filtered odometry, transformed back into lat/long coordinates. The node can also optionally publish the
transform from the UTM frame the the world frame.
-->
<launch>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true">
<rosparam command="load" file="$(find robot_localization)/params/navsat_transform_template.yaml" />
<!-- Placeholders for input remapping. Set your topic names as the "to" values.
<remap from="imu/data" to=""/>
<remap from="odometry/filtered" to=""/>
<remap from="gps/fix" to=""/>
-->
</node>
</launch>

11
Localizations/Packages/robot_localization/launch/ukf_template.launch Normal file
View File

@@ -0,0 +1,11 @@
<launch>
<node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true">
<rosparam command="load" file="$(find robot_localization)/params/ukf_template.yaml" />
<!-- Placeholder for output topic remapping
<remap from="odometry/filtered" to=""/>
<remap from="accel/filtered" to=""/>
-->
</node>
</launch>

27
Localizations/Packages/robot_localization/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,27 @@
<library path="lib/libekf_localization_nodelet">
<class name="RobotLocalization/EkfNodelet"
type="RobotLocalization::EkfNodelet"
base_class_type="nodelet::Nodelet">
<description>
ekf_localization_node as nodelet
</description>
</class>
</library>
<library path="lib/libukf_localization_nodelet">
<class name="RobotLocalization/UkfNodelet"
type="RobotLocalization::UkfNodelet"
base_class_type="nodelet::Nodelet">
<description>
ukf_localization_node as nodelet
</description>
</class>
</library>
<library path="lib/libnavsat_transform_nodelet">
<class name="RobotLocalization/NavSatTransformNodelet"
type="RobotLocalization::NavSatTransformNodelet"
base_class_type="nodelet::Nodelet">
<description>
navsat_transform_node as nodelet
</description>
</class>
</library>

54
Localizations/Packages/robot_localization/package.xml Normal file
View File

@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<package format="3">
<name>robot_localization</name>
<version>2.7.4</version>
<description>Provides nonlinear state estimation through sensor fusion of an abritrary number of sensors.</description>
<maintainer email="ayrton04@gmail.com">Tom Moore</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/robot_localization</url>
<author email="ayrton04@gmail.com">Tom Moore</author>
<buildtool_depend>catkin</buildtool_depend>
<depend>angles</depend>
<depend>cmake_modules</depend>
<depend>diagnostic_msgs</depend>
<depend>diagnostic_updater</depend>
<depend>eigen</depend>
<depend>eigen_conversions</depend>
<depend>geographiclib</depend>
<depend>geographic_msgs</depend>
<depend>geometry_msgs</depend>
<depend>message_filters</depend>
<depend>nav_msgs</depend>
<depend>nodelet</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>yaml-cpp</depend>
<build_depend>message_generation</build_depend>
<build_depend condition="$ROS_PYTHON_VERSION == 2">python-catkin-pkg</build_depend>
<build_depend condition="$ROS_PYTHON_VERSION == 3">python3-catkin-pkg</build_depend>
<build_depend>roslint</build_depend>
<exec_depend>message_runtime</exec_depend>
<test_depend>rosbag</test_depend>
<test_depend>rostest</test_depend>
<test_depend>rosunit</test_depend>
<export>
<rosdoc config="rosdoc.yaml" />
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
</export>
</package>

166
Localizations/Packages/robot_localization/params/dual_ekf_navsat_example.yaml Normal file
View File

@@ -0,0 +1,166 @@
# For parameter descriptions, please refer to the template parameter files for each node.
ekf_se_odom:
frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: odometry/wheel
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
imu0: imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]
ekf_se_map:
frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom0: odometry/wheel
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
odom1: odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
imu0: imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]
initial_estimate_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]
navsat_transform:
frequency: 30
delay: 3.0
magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998
yaw_offset: 1.570796327 # IMU reads 0 facing magnetic north, not east
zero_altitude: false
broadcast_utm_transform: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false

255
Localizations/Packages/robot_localization/params/ekf_template.yaml Normal file
View File

@@ -0,0 +1,255 @@
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30
silent_tf_failure: false
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 0.1
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset: 0.0
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics: true
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0: example/odom
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 2
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
odom0_nodelay: false
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential: false
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
# Further input parameter examples
odom1: example/another_odom
odom1_config: [false, false, true,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2
odom1_pose_rejection_threshold: 2
odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false
pose0: example/pose
pose0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
pose0_differential: true
pose0_relative: false
pose0_queue_size: 5
pose0_rejection_threshold: 2 # Note the difference in parameter name
pose0_nodelay: false
twist0: example/twist
twist0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]
twist0_queue_size: 3
twist0_rejection_threshold: 2
twist0_nodelay: false
imu0: example/imu
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: true
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]

48
Localizations/Packages/robot_localization/params/navsat_transform_template.yaml Normal file
View File

@@ -0,0 +1,48 @@
# Frequency of the main run loop
frequency: 30
# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially
# important if you have use_odometry_yaw set to true. Defaults to 0.
delay: 3.0
# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame.
# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using
# it.
# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it,
# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory.
magnetic_declination_radians: 0
# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it
# doesn't, enter the offset here. Defaults to 0.
yaw_offset: 0.0
# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false.
zero_altitude: false
# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false.
broadcast_utm_transform: false
# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform.
# Note that broadcast_utm_transform still has to be enabled. Defaults to false.
broadcast_utm_transform_as_parent_frame: false
# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as
# /gps/filtered. Defaults to false.
publish_filtered_gps: false
# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the
# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this!
# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw
# if your yaw data is based purely on integrated velocities. Defaults to false.
use_odometry_yaw: false
# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists,
# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message.
wait_for_datum: false
# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the
# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees,
# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east.
datum: [55.944904, -3.186693, 0.0]

267
Localizations/Packages/robot_localization/params/ukf_template.yaml Normal file
View File

@@ -0,0 +1,267 @@
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 0.1
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset: 0.0
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics: true
# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0: example/odom
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 2
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
odom0_nodelay: false
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential: false
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
# Further input parameter examples
odom1: example/another_odom
odom1_config: [false, false, true,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2
odom1_pose_rejection_threshold: 2
odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false
pose0: example/pose
pose0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
pose0_differential: true
pose0_relative: false
pose0_queue_size: 5
pose0_rejection_threshold: 2 # Note the difference in parameter name
pose0_nodelay: false
twist0: example/twist
twist0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]
twist0_queue_size: 3
twist0_rejection_threshold: 2
twist0_nodelay: false
imu0: example/imu
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: true
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
# [ADVANCED, UKF ONLY] The alpha and kappa variables control the spread of the sigma points. Unless you are familiar
# with UKFs, it's probably a good idea to leave these alone.
# Defaults to 0.001 if unspecified.
alpha: 0.001
# Defaults to 0 if unspecified.
kappa: 0
# [ADVANCED, UKF ONLY] The beta variable relates to the distribution of the state vector. Again, it's probably best to
# leave this alone if you're uncertain. Defaults to 2 if unspecified.
beta: 2

6
Localizations/Packages/robot_localization/rosdoc.yaml Normal file
View File

@@ -0,0 +1,6 @@
- builder: sphinx
sphinx_root_dir: doc
- builder: doxygen
output_dir: api
file_patterns: '*.cpp *.h *.dox *.md'
exclude_patterns: '*/test/*'

389
Localizations/Packages/robot_localization/src/ekf.cpp Normal file
View File

@@ -0,0 +1,389 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ekf.h"
#include "robot_localization/filter_common.h"
#include <XmlRpcException.h>
#include <iomanip>
#include <limits>
#include <sstream>
#include <vector>
namespace RobotLocalization
{
Ekf::Ekf(std::vector<double>) :
FilterBase() // Must initialize filter base!
{
}
Ekf::~Ekf()
{
}
void Ekf::correct(const Measurement &measurement)
{
FB_DEBUG("---------------------- Ekf::correct ----------------------\n" <<
"State is:\n" << state_ << "\n"
"Topic is:\n" << measurement.topicName_ << "\n"
"Measurement is:\n" << measurement.measurement_ << "\n"
"Measurement topic name is:\n" << measurement.topicName_ << "\n\n"
"Measurement covariance is:\n" << measurement.covariance_ << "\n");
// We don't want to update everything, so we need to build matrices that only update
// the measured parts of our state vector. Throughout prediction and correction, we
// attempt to maximize efficiency in Eigen.
// First, determine how many state vector values we're updating
std::vector<size_t> updateIndices;
for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
{
if (measurement.updateVector_[i])
{
// Handle nan and inf values in measurements
if (std::isnan(measurement.measurement_(i)))
{
FB_DEBUG("Value at index " << i << " was nan. Excluding from update.\n");
}
else if (std::isinf(measurement.measurement_(i)))
{
FB_DEBUG("Value at index " << i << " was inf. Excluding from update.\n");
}
else
{
updateIndices.push_back(i);
}
}
}
FB_DEBUG("Update indices are:\n" << updateIndices << "\n");
size_t updateSize = updateIndices.size();
// Now set up the relevant matrices
Eigen::VectorXd stateSubset(updateSize); // x (in most literature)
Eigen::VectorXd measurementSubset(updateSize); // z
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // R
Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); // H
Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize); // K
Eigen::VectorXd innovationSubset(updateSize); // z - Hx
stateSubset.setZero();
measurementSubset.setZero();
measurementCovarianceSubset.setZero();
stateToMeasurementSubset.setZero();
kalmanGainSubset.setZero();
innovationSubset.setZero();
// Now build the sub-matrices from the full-sized matrices
for (size_t i = 0; i < updateSize; ++i)
{
measurementSubset(i) = measurement.measurement_(updateIndices[i]);
stateSubset(i) = state_(updateIndices[i]);
for (size_t j = 0; j < updateSize; ++j)
{
measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]);
}
// Handle negative (read: bad) covariances in the measurement. Rather
// than exclude the measurement or make up a covariance, just take
// the absolute value.
if (measurementCovarianceSubset(i, i) < 0)
{
FB_DEBUG("WARNING: Negative covariance for index " << i <<
" of measurement (value is" << measurementCovarianceSubset(i, i) <<
"). Using absolute value...\n");
measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
}
// If the measurement variance for a given variable is very
// near 0 (as in e-50 or so) and the variance for that
// variable in the covariance matrix is also near zero, then
// the Kalman gain computation will blow up. Really, no
// measurement can be completely without error, so add a small
// amount in that case.
if (measurementCovarianceSubset(i, i) < 1e-9)
{
FB_DEBUG("WARNING: measurement had very small error covariance for index " << updateIndices[i] <<
". Adding some noise to maintain filter stability.\n");
measurementCovarianceSubset(i, i) = 1e-9;
}
}
// The state-to-measurement function, h, will now be a measurement_size x full_state_size
// matrix, with ones in the (i, i) locations of the values to be updated
for (size_t i = 0; i < updateSize; ++i)
{
stateToMeasurementSubset(i, updateIndices[i]) = 1;
}
FB_DEBUG("Current state subset is:\n" << stateSubset <<
"\nMeasurement subset is:\n" << measurementSubset <<
"\nMeasurement covariance subset is:\n" << measurementCovarianceSubset <<
"\nState-to-measurement subset is:\n" << stateToMeasurementSubset << "\n");
// (1) Compute the Kalman gain: K = (PH') / (HPH' + R)
Eigen::MatrixXd pht = estimateErrorCovariance_ * stateToMeasurementSubset.transpose();
Eigen::MatrixXd hphrInv = (stateToMeasurementSubset * pht + measurementCovarianceSubset).inverse();
kalmanGainSubset.noalias() = pht * hphrInv;
innovationSubset = (measurementSubset - stateSubset);
// Wrap angles in the innovation
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll ||
updateIndices[i] == StateMemberPitch ||
updateIndices[i] == StateMemberYaw)
{
while (innovationSubset(i) < -PI)
{
innovationSubset(i) += TAU;
}
while (innovationSubset(i) > PI)
{
innovationSubset(i) -= TAU;
}
}
}
// (2) Check Mahalanobis distance between mapped measurement and state.
if (checkMahalanobisThreshold(innovationSubset, hphrInv, measurement.mahalanobisThresh_))
{
// (3) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
state_.noalias() += kalmanGainSubset * innovationSubset;
// (4) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK'
Eigen::MatrixXd gainResidual = identity_;
gainResidual.noalias() -= kalmanGainSubset * stateToMeasurementSubset;
estimateErrorCovariance_ = gainResidual * estimateErrorCovariance_ * gainResidual.transpose();
estimateErrorCovariance_.noalias() += kalmanGainSubset *
measurementCovarianceSubset *
kalmanGainSubset.transpose();
// Handle wrapping of angles
wrapStateAngles();
FB_DEBUG("Kalman gain subset is:\n" << kalmanGainSubset <<
"\nInnovation is:\n" << innovationSubset <<
"\nCorrected full state is:\n" << state_ <<
"\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n---------------------- /Ekf::correct ----------------------\n");
}
}
void Ekf::predict(const double referenceTime, const double delta)
{
FB_DEBUG("---------------------- Ekf::predict ----------------------\n" <<
"delta is " << delta << "\n" <<
"state is " << state_ << "\n");
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
double pitchVel = state_(StateMemberVpitch);
double yawVel = state_(StateMemberVyaw);
double xAcc = state_(StateMemberAx);
double yAcc = state_(StateMemberAy);
double zAcc = state_(StateMemberAz);
// We'll need these trig calculations a lot.
double sp = ::sin(pitch);
double cp = ::cos(pitch);
double cpi = 1.0 / cp;
double tp = sp * cpi;
double sr = ::sin(roll);
double cr = ::cos(roll);
double sy = ::sin(yaw);
double cy = ::cos(yaw);
prepareControl(referenceTime, delta);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double xCoeff = 0.0;
double yCoeff = 0.0;
double zCoeff = 0.0;
double oneHalfATSquared = 0.5 * delta * delta;
yCoeff = cy * sp * cr + sy * sr;
zCoeff = -cy * sp * sr + sy * cr;
double dFx_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dR = 1.0 + (cr * tp * pitchVel - sr * tp * yawVel) * delta;
xCoeff = -cy * sp;
yCoeff = cy * cp * sr;
zCoeff = cy * cp * cr;
double dFx_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dP = (cpi * cpi * sr * pitchVel + cpi * cpi * cr * yawVel) * delta;
xCoeff = -sy * cp;
yCoeff = -sy * sp * sr - cy * cr;
zCoeff = -sy * sp * cr + cy * sr;
double dFx_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = sy * sp * cr - cy * sr;
zCoeff = -sy * sp * sr - cy * cr;
double dFy_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFP_dR = (-sr * pitchVel - cr * yawVel) * delta;
xCoeff = -sy * sp;
yCoeff = sy * cp * sr;
zCoeff = sy * cp * cr;
double dFy_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = cy * cp;
yCoeff = cy * sp * sr - sy * cr;
zCoeff = cy * sp * cr + sy * sr;
double dFy_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = cp * cr;
zCoeff = -cp * sr;
double dFz_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFY_dR = (cr * cpi * pitchVel - sr * cpi * yawVel) * delta;
xCoeff = -cp;
yCoeff = -sp * sr;
zCoeff = -sp * cr;
double dFz_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFY_dP = (sr * tp * cpi * pitchVel + cr * tp * cpi * yawVel) * delta;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dFx_dR;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dFx_dP;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dFx_dY;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dFy_dR;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dFy_dP;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dFy_dY;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dFz_dR;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dFz_dP;
transferFunctionJacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR;
transferFunctionJacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP;
transferFunctionJacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR;
transferFunctionJacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR;
transferFunctionJacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP;
FB_DEBUG("Transfer function is:\n" << transferFunction_ <<
"\nTransfer function Jacobian is:\n" << transferFunctionJacobian_ <<
"\nProcess noise covariance is:\n" << processNoiseCovariance_ <<
"\nCurrent state is:\n" << state_ << "\n");
Eigen::MatrixXd *processNoiseCovariance = &processNoiseCovariance_;
if (useDynamicProcessNoiseCovariance_)
{
computeDynamicProcessNoiseCovariance(state_, delta);
processNoiseCovariance = &dynamicProcessNoiseCovariance_;
}
// (1) Apply control terms, which are actually accelerations
state_(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;
state_(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;
state_(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;
state_(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?
controlAcceleration_(ControlMemberVx) : state_(StateMemberAx));
state_(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?
controlAcceleration_(ControlMemberVy) : state_(StateMemberAy));
state_(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?
controlAcceleration_(ControlMemberVz) : state_(StateMemberAz));
// (2) Project the state forward: x = Ax + Bu (really, x = f(x, u))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
FB_DEBUG("Predicted state is:\n" << state_ <<
"\nCurrent estimate error covariance is:\n" << estimateErrorCovariance_ << "\n");
// (3) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ *
estimateErrorCovariance_ *
transferFunctionJacobian_.transpose());
estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);
FB_DEBUG("Predicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n--------------------- /Ekf::predict ----------------------\n");
}
} // namespace RobotLocalization

51
Localizations/Packages/robot_localization/src/ekf_localization_node.cpp Normal file
View File

@@ -0,0 +1,51 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <cstdlib>
#include <ros/ros.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "ekf_navigation_node");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
RobotLocalization::RosEkf ekf(nh, nh_priv);
ekf.initialize();
ros::spin();
return EXIT_SUCCESS;
}

64
Localizations/Packages/robot_localization/src/ekf_localization_nodelet.cpp Normal file
View File

@@ -0,0 +1,64 @@
/*
* Copyright (c) 2017 Simon Gene Gottlieb
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <memory>
namespace RobotLocalization
{
class EkfNodelet : public nodelet::Nodelet
{
private:
std::unique_ptr<RosEkf> ekf;
public:
virtual void onInit()
{
NODELET_DEBUG("Initializing nodelet...");
ros::NodeHandle nh = getNodeHandle();
ros::NodeHandle nh_priv = getPrivateNodeHandle();
ekf = std::make_unique<RosEkf>(nh, nh_priv, getName());
ekf->initialize();
}
};
} // namespace RobotLocalization
PLUGINLIB_EXPORT_CLASS(RobotLocalization::EkfNodelet, nodelet::Nodelet);

403
Localizations/Packages/robot_localization/src/filter_base.cpp Normal file
View File

@@ -0,0 +1,403 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/filter_base.h"
#include "robot_localization/filter_common.h"
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
#include <vector>
namespace RobotLocalization
{
FilterBase::FilterBase() :
initialized_(false),
useControl_(false),
useDynamicProcessNoiseCovariance_(false),
lastMeasurementTime_(0.0),
latestControlTime_(0.0),
controlTimeout_(0.0),
sensorTimeout_(0.0),
controlUpdateVector_(TWIST_SIZE, 0),
accelerationGains_(TWIST_SIZE, 0.0),
accelerationLimits_(TWIST_SIZE, 0.0),
decelerationGains_(TWIST_SIZE, 0.0),
decelerationLimits_(TWIST_SIZE, 0.0),
controlAcceleration_(TWIST_SIZE),
latestControl_(TWIST_SIZE),
predictedState_(STATE_SIZE),
state_(STATE_SIZE),
covarianceEpsilon_(STATE_SIZE, STATE_SIZE),
dynamicProcessNoiseCovariance_(STATE_SIZE, STATE_SIZE),
estimateErrorCovariance_(STATE_SIZE, STATE_SIZE),
identity_(STATE_SIZE, STATE_SIZE),
processNoiseCovariance_(STATE_SIZE, STATE_SIZE),
transferFunction_(STATE_SIZE, STATE_SIZE),
transferFunctionJacobian_(STATE_SIZE, STATE_SIZE),
debugStream_(NULL),
debug_(false)
{
reset();
}
FilterBase::~FilterBase()
{
}
void FilterBase::reset()
{
initialized_ = false;
// Clear the state and predicted state
state_.setZero();
predictedState_.setZero();
controlAcceleration_.setZero();
// Prepare the invariant parts of the transfer
// function
transferFunction_.setIdentity();
// Clear the Jacobian
transferFunctionJacobian_.setZero();
// Set the estimate error covariance. We want our measurements
// to be accepted rapidly when the filter starts, so we should
// initialize the state's covariance with large values.
estimateErrorCovariance_.setIdentity();
estimateErrorCovariance_ *= 1e-9;
// We need the identity for the update equations
identity_.setIdentity();
// Set the epsilon matrix to be a matrix with small values on the diagonal
// It is used to maintain the positive-definite property of the covariance
covarianceEpsilon_.setIdentity();
covarianceEpsilon_ *= 0.001;
// Assume 30Hz from sensor data (configurable)
sensorTimeout_ = 0.033333333;
// Initialize our measurement time
lastMeasurementTime_ = 0;
// These can be overridden via the launch parameters,
// but we need default values.
processNoiseCovariance_.setZero();
processNoiseCovariance_(StateMemberX, StateMemberX) = 0.05;
processNoiseCovariance_(StateMemberY, StateMemberY) = 0.05;
processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.06;
processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.04;
processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.01;
processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.01;
processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.02;
processNoiseCovariance_(StateMemberAx, StateMemberAx) = 0.01;
processNoiseCovariance_(StateMemberAy, StateMemberAy) = 0.01;
processNoiseCovariance_(StateMemberAz, StateMemberAz) = 0.015;
dynamicProcessNoiseCovariance_ = processNoiseCovariance_;
}
void FilterBase::computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta)
{
// A more principled approach would be to get the current velocity from the state, make a diagonal matrix from it,
// and then rotate it to be in the world frame (i.e., the same frame as the pose data). We could then use this
// rotated velocity matrix to scale the process noise covariance for the pose variables as
// rotatedVelocityMatrix * poseCovariance * rotatedVelocityMatrix'
// However, this presents trouble for robots that may incur rotational error as a result of linear motion (and
// vice-versa). Instead, we create a diagonal matrix whose diagonal values are the vector norm of the state's
// velocity. We use that to scale the process noise covariance.
Eigen::MatrixXd velocityMatrix(TWIST_SIZE, TWIST_SIZE);
velocityMatrix.setIdentity();
velocityMatrix.diagonal() *= state.segment(POSITION_V_OFFSET, TWIST_SIZE).norm();
dynamicProcessNoiseCovariance_.block<TWIST_SIZE, TWIST_SIZE>(POSITION_OFFSET, POSITION_OFFSET) =
velocityMatrix *
processNoiseCovariance_.block<TWIST_SIZE, TWIST_SIZE>(POSITION_OFFSET, POSITION_OFFSET) *
velocityMatrix.transpose();
}
const Eigen::VectorXd& FilterBase::getControl()
{
return latestControl_;
}
double FilterBase::getControlTime()
{
return latestControlTime_;
}
bool FilterBase::getDebug()
{
return debug_;
}
const Eigen::MatrixXd& FilterBase::getEstimateErrorCovariance()
{
return estimateErrorCovariance_;
}
bool FilterBase::getInitializedStatus()
{
return initialized_;
}
double FilterBase::getLastMeasurementTime()
{
return lastMeasurementTime_;
}
const Eigen::VectorXd& FilterBase::getPredictedState()
{
return predictedState_;
}
const Eigen::MatrixXd& FilterBase::getProcessNoiseCovariance()
{
return processNoiseCovariance_;
}
double FilterBase::getSensorTimeout()
{
return sensorTimeout_;
}
const Eigen::VectorXd& FilterBase::getState()
{
return state_;
}
void FilterBase::processMeasurement(const Measurement &measurement)
{
FB_DEBUG("------ FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n");
double delta = 0.0;
// If we've had a previous reading, then go through the predict/update
// cycle. Otherwise, set our state and covariance to whatever we get
// from this measurement.
if (initialized_)
{
// Determine how much time has passed since our last measurement
delta = measurement.time_ - lastMeasurementTime_;
FB_DEBUG("Filter is already initialized. Carrying out predict/correct loop...\n"
"Measurement time is " << std::setprecision(20) << measurement.time_ <<
", last measurement time is " << lastMeasurementTime_ << ", delta is " << delta << "\n");
// Only want to carry out a prediction if it's
// forward in time. Otherwise, just correct.
if (delta > 0)
{
validateDelta(delta);
predict(measurement.time_, delta);
// Return this to the user
predictedState_ = state_;
}
correct(measurement);
}
else
{
FB_DEBUG("First measurement. Initializing filter.\n");
// Initialize the filter, but only with the values we're using
size_t measurementLength = measurement.updateVector_.size();
for (size_t i = 0; i < measurementLength; ++i)
{
state_[i] = (measurement.updateVector_[i] ? measurement.measurement_[i] : state_[i]);
}
// Same for covariance
for (size_t i = 0; i < measurementLength; ++i)
{
for (size_t j = 0; j < measurementLength; ++j)
{
estimateErrorCovariance_(i, j) = (measurement.updateVector_[i] && measurement.updateVector_[j] ?
measurement.covariance_(i, j) :
estimateErrorCovariance_(i, j));
}
}
initialized_ = true;
}
if (delta >= 0.0)
{
lastMeasurementTime_ = measurement.time_;
}
FB_DEBUG("------ /FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n");
}
void FilterBase::setControl(const Eigen::VectorXd &control, const double controlTime)
{
latestControl_ = control;
latestControlTime_ = controlTime;
}
void FilterBase::setControlParams(const std::vector<int> &updateVector, const double controlTimeout,
const std::vector<double> &accelerationLimits, const std::vector<double> &accelerationGains,
const std::vector<double> &decelerationLimits, const std::vector<double> &decelerationGains)
{
useControl_ = true;
controlUpdateVector_ = updateVector;
controlTimeout_ = controlTimeout;
accelerationLimits_ = accelerationLimits;
accelerationGains_ = accelerationGains;
decelerationLimits_ = decelerationLimits;
decelerationGains_ = decelerationGains;
}
void FilterBase::setDebug(const bool debug, std::ostream *outStream)
{
if (debug)
{
if (outStream != NULL)
{
debugStream_ = outStream;
debug_ = true;
}
else
{
debug_ = false;
}
}
else
{
debug_ = false;
}
}
void FilterBase::setUseDynamicProcessNoiseCovariance(const bool useDynamicProcessNoiseCovariance)
{
useDynamicProcessNoiseCovariance_ = useDynamicProcessNoiseCovariance;
}
void FilterBase::setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance)
{
estimateErrorCovariance_ = estimateErrorCovariance;
}
void FilterBase::setLastMeasurementTime(const double lastMeasurementTime)
{
lastMeasurementTime_ = lastMeasurementTime;
}
void FilterBase::setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance)
{
processNoiseCovariance_ = processNoiseCovariance;
dynamicProcessNoiseCovariance_ = processNoiseCovariance_;
}
void FilterBase::setSensorTimeout(const double sensorTimeout)
{
sensorTimeout_ = sensorTimeout;
}
void FilterBase::setState(const Eigen::VectorXd &state)
{
state_ = state;
}
void FilterBase::validateDelta(double &delta)
{
// This handles issues with ROS time when use_sim_time is on and we're playing from bags.
if (delta > 100000.0)
{
FB_DEBUG("Delta was very large. Suspect playing from bag file. Setting to 0.01\n");
delta = 0.01;
}
}
void FilterBase::prepareControl(const double referenceTime, const double predictionDelta)
{
controlAcceleration_.setZero();
if (useControl_)
{
bool timedOut = ::fabs(referenceTime - latestControlTime_) >= controlTimeout_;
if (timedOut)
{
FB_DEBUG("Control timed out. Reference time was " << referenceTime << ", latest control time was " <<
latestControlTime_ << ", control timeout was " << controlTimeout_ << "\n");
}
for (size_t controlInd = 0; controlInd < TWIST_SIZE; ++controlInd)
{
if (controlUpdateVector_[controlInd])
{
controlAcceleration_(controlInd) = computeControlAcceleration(state_(controlInd + POSITION_V_OFFSET),
(timedOut ? 0.0 : latestControl_(controlInd)), accelerationLimits_[controlInd],
accelerationGains_[controlInd], decelerationLimits_[controlInd], decelerationGains_[controlInd]);
}
}
}
}
void FilterBase::wrapStateAngles()
{
state_(StateMemberRoll) = FilterUtilities::clampRotation(state_(StateMemberRoll));
state_(StateMemberPitch) = FilterUtilities::clampRotation(state_(StateMemberPitch));
state_(StateMemberYaw) = FilterUtilities::clampRotation(state_(StateMemberYaw));
}
bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
const Eigen::MatrixXd &invCovariance,
const double nsigmas)
{
double sqMahalanobis = innovation.dot(invCovariance * innovation);
double threshold = nsigmas * nsigmas;
if (sqMahalanobis >= threshold)
{
FB_DEBUG("Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis << "\n" <<
"Threshold is: " << threshold << "\n" <<
"Innovation is: " << innovation << "\n" <<
"Innovation covariance is:\n" << invCovariance << "\n");
return false;
}
return true;
}
} // namespace RobotLocalization

146
Localizations/Packages/robot_localization/src/filter_utilities.cpp Normal file
View File

@@ -0,0 +1,146 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/filter_utilities.h"
#include "robot_localization/filter_common.h"
#include <string>
#include <vector>
std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat)
{
os << "[";
int rowCount = static_cast<int>(mat.rows());
for (int row = 0; row < rowCount; ++row)
{
if (row > 0)
{
os << " ";
}
for (int col = 0; col < mat.cols(); ++col)
{
os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << mat(row, col);
}
if (row < rowCount - 1)
{
os << "\n";
}
}
os << "]\n";
return os;
}
std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec)
{
os << "[";
for (int dim = 0; dim < vec.rows(); ++dim)
{
os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec(dim);
}
os << "]\n";
return os;
}
std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec)
{
os << "[";
for (size_t dim = 0; dim < vec.size(); ++dim)
{
os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec[dim];
}
os << "]\n";
return os;
}
std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec)
{
os << "[";
for (size_t dim = 0; dim < vec.size(); ++dim)
{
os << std::setiosflags(std::ios::left) << std::setw(3) << (vec[dim] ? "t" : "f");
}
os << "]\n";
return os;
}
namespace RobotLocalization
{
namespace FilterUtilities
{
void appendPrefix(std::string tfPrefix, std::string &frameId)
{
// Strip all leading slashes for tf2 compliance
if (!frameId.empty() && frameId.at(0) == '/')
{
frameId = frameId.substr(1);
}
if (!tfPrefix.empty() && tfPrefix.at(0) == '/')
{
tfPrefix = tfPrefix.substr(1);
}
// If we do have a tf prefix, then put a slash in between
if (!tfPrefix.empty())
{
frameId = tfPrefix + "/" + frameId;
}
}
double clampRotation(double rotation)
{
while (rotation > PI)
{
rotation -= TAU;
}
while (rotation < -PI)
{
rotation += TAU;
}
return rotation;
}
} // namespace FilterUtilities
} // namespace RobotLocalization

915
Localizations/Packages/robot_localization/src/navsat_transform.cpp Normal file
View File

@@ -0,0 +1,915 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/navsat_transform.h"
#include "robot_localization/filter_common.h"
#include "robot_localization/filter_utilities.h"
#include "robot_localization/navsat_conversions.h"
#include "robot_localization/ros_filter_utilities.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <XmlRpcException.h>
#include <string>
namespace RobotLocalization
{
NavSatTransform::NavSatTransform(ros::NodeHandle nh, ros::NodeHandle nh_priv) :
broadcast_cartesian_transform_(false),
broadcast_cartesian_transform_as_parent_frame_(false),
gps_updated_(false),
has_transform_gps_(false),
has_transform_imu_(false),
has_transform_odom_(false),
odom_updated_(false),
publish_gps_(false),
transform_good_(false),
use_manual_datum_(false),
use_odometry_yaw_(false),
use_local_cartesian_(false),
zero_altitude_(false),
magnetic_declination_(0.0),
yaw_offset_(0.0),
base_link_frame_id_("base_link"),
gps_frame_id_(""),
utm_zone_(0),
world_frame_id_("odom"),
transform_timeout_(ros::Duration(0)),
tf_listener_(tf_buffer_)
{
ROS_INFO("Waiting for valid clock time...");
ros::Time::waitForValid();
ROS_INFO("Valid clock time received. Starting node.");
latest_cartesian_covariance_.resize(POSE_SIZE, POSE_SIZE);
latest_odom_covariance_.resize(POSE_SIZE, POSE_SIZE);
double frequency;
double delay = 0.0;
double transform_timeout = 0.0;
// Load the parameters we need
nh_priv.getParam("magnetic_declination_radians", magnetic_declination_);
nh_priv.param("yaw_offset", yaw_offset_, 0.0);
nh_priv.param("broadcast_cartesian_transform", broadcast_cartesian_transform_, false);
nh_priv.param("broadcast_cartesian_transform_as_parent_frame",
broadcast_cartesian_transform_as_parent_frame_, false);
nh_priv.param("zero_altitude", zero_altitude_, false);
nh_priv.param("publish_filtered_gps", publish_gps_, false);
nh_priv.param("use_odometry_yaw", use_odometry_yaw_, false);
nh_priv.param("wait_for_datum", use_manual_datum_, false);
nh_priv.param("use_local_cartesian", use_local_cartesian_, false);
nh_priv.param("frequency", frequency, 10.0);
nh_priv.param("delay", delay, 0.0);
nh_priv.param("transform_timeout", transform_timeout, 0.0);
nh_priv.param("cartesian_frame_id", cartesian_frame_id_, std::string(use_local_cartesian_ ? "local_enu" : "utm"));
transform_timeout_.fromSec(transform_timeout);
// Check for deprecated parameters
if (nh_priv.getParam("broadcast_utm_transform", broadcast_cartesian_transform_))
{
ROS_WARN("navsat_transform, Parameter 'broadcast_utm_transform' has been deprecated. Please use"
"'broadcast_cartesian_transform' instead.");
}
if (nh_priv.getParam("broadcast_utm_transform_as_parent_frame", broadcast_cartesian_transform_as_parent_frame_))
{
ROS_WARN("navsat_transform, Parameter 'broadcast_utm_transform_as_parent_frame' has been deprecated. Please use"
"'broadcast_cartesian_transform_as_parent_frame' instead.");
}
// Check if tf warnings should be suppressed
nh.getParam("/silent_tf_failure", tf_silent_failure_);
// Subscribe to the messages and services we need
datum_srv_ = nh.advertiseService("datum", &NavSatTransform::datumCallback, this);
to_ll_srv_ = nh.advertiseService("toLL", &NavSatTransform::toLLCallback, this);
from_ll_srv_ = nh.advertiseService("fromLL", &NavSatTransform::fromLLCallback, this);
set_utm_zone_srv_ = nh.advertiseService("setUTMZone", &NavSatTransform::setUTMZoneCallback, this);
if (use_manual_datum_ && nh_priv.hasParam("datum"))
{
XmlRpc::XmlRpcValue datum_config;
try
{
double datum_lat;
double datum_lon;
double datum_yaw;
nh_priv.getParam("datum", datum_config);
// Handle datum specification. Users should always specify a baseLinkFrameId_ in the
// datum config, but we had a release where it wasn't used, so we'll maintain compatibility.
ROS_ASSERT(datum_config.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(datum_config.size() >= 3);
if (datum_config.size() > 3)
{
ROS_WARN_STREAM("Deprecated datum parameter configuration detected. Only the first three parameters "
"(latitude, longitude, yaw) will be used. frame_ids will be derived from odometry and navsat inputs.");
}
std::ostringstream ostr;
ostr << std::setprecision(20) << datum_config[0] << " " << datum_config[1] << " " << datum_config[2];
std::istringstream istr(ostr.str());
istr >> datum_lat >> datum_lon >> datum_yaw;
// Try to resolve tf_prefix
std::string tf_prefix = "";
std::string tf_prefix_path = "";
if (nh_priv.searchParam("tf_prefix", tf_prefix_path))
{
nh_priv.getParam(tf_prefix_path, tf_prefix);
}
// Append the tf prefix in a tf2-friendly manner
FilterUtilities::appendPrefix(tf_prefix, world_frame_id_);
FilterUtilities::appendPrefix(tf_prefix, base_link_frame_id_);
robot_localization::SetDatum::Request request;
request.geo_pose.position.latitude = datum_lat;
request.geo_pose.position.longitude = datum_lon;
request.geo_pose.position.altitude = 0.0;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, datum_yaw);
request.geo_pose.orientation = tf2::toMsg(quat);
robot_localization::SetDatum::Response response;
datumCallback(request, response);
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() <<
" for process_noise_covariance (type: " << datum_config.getType() << ")");
}
}
odom_sub_ = nh.subscribe("odometry/filtered", 1, &NavSatTransform::odomCallback, this);
gps_sub_ = nh.subscribe("gps/fix", 1, &NavSatTransform::gpsFixCallback, this);
if (!use_odometry_yaw_ && !use_manual_datum_)
{
imu_sub_ = nh.subscribe("imu/data", 1, &NavSatTransform::imuCallback, this);
}
gps_odom_pub_ = nh.advertise<nav_msgs::Odometry>("odometry/gps", 10);
if (publish_gps_)
{
filtered_gps_pub_ = nh.advertise<sensor_msgs::NavSatFix>("gps/filtered", 10);
}
// Sleep for the parameterized amount of time, to give
// other nodes time to start up (not always necessary)
ros::Duration start_delay(delay);
start_delay.sleep();
periodicUpdateTimer_ = nh.createTimer(ros::Duration(1./frequency), &NavSatTransform::periodicUpdate, this);
}
NavSatTransform::~NavSatTransform()
{
}
// void NavSatTransform::run()
void NavSatTransform::periodicUpdate(const ros::TimerEvent& event)
{
if (!transform_good_)
{
computeTransform();
if (transform_good_ && !use_odometry_yaw_ && !use_manual_datum_)
{
// Once we have the transform, we don't need the IMU
imu_sub_.shutdown();
}
}
else
{
nav_msgs::Odometry gps_odom;
if (prepareGpsOdometry(gps_odom))
{
gps_odom_pub_.publish(gps_odom);
}
if (publish_gps_)
{
sensor_msgs::NavSatFix odom_gps;
if (prepareFilteredGps(odom_gps))
{
filtered_gps_pub_.publish(odom_gps);
}
}
}
}
void NavSatTransform::computeTransform()
{
// Only do this if:
// 1. We haven't computed the odom_frame->cartesian_frame transform before
// 2. We've received the data we need
if (!transform_good_ &&
has_transform_odom_ &&
has_transform_gps_ &&
has_transform_imu_)
{
// The cartesian pose we have is given at the location of the GPS sensor on the robot. We need to get the
// cartesian pose of the robot's origin.
tf2::Transform transform_cartesian_pose_corrected;
if (!use_manual_datum_)
{
getRobotOriginCartesianPose(transform_cartesian_pose_, transform_cartesian_pose_corrected, ros::Time(0));
}
else
{
transform_cartesian_pose_corrected = transform_cartesian_pose_;
}
// Get the IMU's current RPY values. Need the raw values (for yaw, anyway).
tf2::Matrix3x3 mat(transform_orientation_);
// Convert to RPY
double imu_roll;
double imu_pitch;
double imu_yaw;
mat.getRPY(imu_roll, imu_pitch, imu_yaw);
/* The IMU's heading was likely originally reported w.r.t. magnetic north.
* However, all the nodes in robot_localization assume that orientation data,
* including that reported by IMUs, is reported in an ENU frame, with a 0 yaw
* value being reported when facing east and increasing counter-clockwise (i.e.,
* towards north). To make the world frame ENU aligned, where X is east
* and Y is north, we have to take into account three additional considerations:
* 1. The IMU may have its non-ENU frame data transformed to ENU, but there's
* a possibility that its data has not been corrected for magnetic
* declination. We need to account for this. A positive magnetic
* declination is counter-clockwise in an ENU frame. Therefore, if
* we have a magnetic declination of N radians, then when the sensor
* is facing a heading of N, it reports 0. Therefore, we need to add
* the declination angle.
* 2. To account for any other offsets that may not be accounted for by the
* IMU driver or any interim processing node, we expose a yaw offset that
* lets users work with navsat_transform_node.
* 3. UTM grid isn't aligned with True East\North. To account for the difference
* we need to add meridian convergence angle when using UTM. This value will be
* 0.0 when use_local_cartesian is TRUE.
*/
imu_yaw += (magnetic_declination_ + yaw_offset_ + utm_meridian_convergence_);
ROS_INFO_STREAM("Corrected for magnetic declination of " << std::fixed << magnetic_declination_ <<
", user-specified offset of " << yaw_offset_ <<
" and meridian convergence of " << utm_meridian_convergence_ << "." <<
" Transform heading factor is now " << imu_yaw);
// Convert to tf-friendly structures
tf2::Quaternion imu_quat;
imu_quat.setRPY(0.0, 0.0, imu_yaw);
// The transform order will be orig_odom_pos * orig_cartesian_pos_inverse * cur_cartesian_pos.
// Doing it this way will allow us to cope with having non-zero odometry position
// when we get our first GPS message.
tf2::Transform cartesian_pose_with_orientation;
cartesian_pose_with_orientation.setOrigin(transform_cartesian_pose_corrected.getOrigin());
cartesian_pose_with_orientation.setRotation(imu_quat);
// Remove roll and pitch from odometry pose
// Must be done because roll and pitch is removed from cartesian_pose_with_orientation
double odom_roll, odom_pitch, odom_yaw;
tf2::Matrix3x3(transform_world_pose_.getRotation()).getRPY(odom_roll, odom_pitch, odom_yaw);
tf2::Quaternion odom_quat;
odom_quat.setRPY(0.0, 0.0, odom_yaw);
tf2::Transform transform_world_pose_yaw_only(transform_world_pose_);
transform_world_pose_yaw_only.setRotation(odom_quat);
cartesian_world_transform_.mult(transform_world_pose_yaw_only, cartesian_pose_with_orientation.inverse());
cartesian_world_trans_inverse_ = cartesian_world_transform_.inverse();
ROS_INFO_STREAM("Transform world frame pose is: " << transform_world_pose_);
ROS_INFO_STREAM("World frame->cartesian transform is " << cartesian_world_transform_);
transform_good_ = true;
// Send out the (static) Cartesian transform in case anyone else would like to use it.
if (broadcast_cartesian_transform_)
{
geometry_msgs::TransformStamped cartesian_transform_stamped;
cartesian_transform_stamped.header.stamp = ros::Time::now();
cartesian_transform_stamped.header.frame_id = (broadcast_cartesian_transform_as_parent_frame_ ?
cartesian_frame_id_ : world_frame_id_);
cartesian_transform_stamped.child_frame_id = (broadcast_cartesian_transform_as_parent_frame_ ?
world_frame_id_ : cartesian_frame_id_);
cartesian_transform_stamped.transform = (broadcast_cartesian_transform_as_parent_frame_ ?
tf2::toMsg(cartesian_world_trans_inverse_) :
tf2::toMsg(cartesian_world_transform_));
cartesian_transform_stamped.transform.translation.z = (zero_altitude_ ?
0.0 : cartesian_transform_stamped.transform.translation.z);
cartesian_broadcaster_.sendTransform(cartesian_transform_stamped);
}
}
}
bool NavSatTransform::datumCallback(robot_localization::SetDatum::Request& request,
robot_localization::SetDatum::Response&)
{
// If we get a service call with a manual datum, even if we already computed the transform using the robot's
// initial pose, then we want to assume that we are using a datum from now on, and we want other methods to
// not attempt to transform the values we are specifying here.
use_manual_datum_ = true;
transform_good_ = false;
sensor_msgs::NavSatFix *fix = new sensor_msgs::NavSatFix();
fix->latitude = request.geo_pose.position.latitude;
fix->longitude = request.geo_pose.position.longitude;
fix->altitude = request.geo_pose.position.altitude;
fix->header.stamp = ros::Time::now();
fix->position_covariance[0] = 0.1;
fix->position_covariance[4] = 0.1;
fix->position_covariance[8] = 0.1;
fix->position_covariance_type = sensor_msgs::NavSatStatus::STATUS_FIX;
sensor_msgs::NavSatFixConstPtr fix_ptr(fix);
setTransformGps(fix_ptr);
nav_msgs::Odometry *odom = new nav_msgs::Odometry();
odom->pose.pose.orientation.x = 0;
odom->pose.pose.orientation.y = 0;
odom->pose.pose.orientation.z = 0;
odom->pose.pose.orientation.w = 1;
odom->pose.pose.position.x = 0;
odom->pose.pose.position.y = 0;
odom->pose.pose.position.z = 0;
odom->header.frame_id = world_frame_id_;
odom->child_frame_id = base_link_frame_id_;
nav_msgs::OdometryConstPtr odom_ptr(odom);
setTransformOdometry(odom_ptr);
sensor_msgs::Imu *imu = new sensor_msgs::Imu();
imu->orientation = request.geo_pose.orientation;
imu->header.frame_id = base_link_frame_id_;
sensor_msgs::ImuConstPtr imu_ptr(imu);
imuCallback(imu_ptr);
return true;
}
bool NavSatTransform::toLLCallback(robot_localization::ToLL::Request& request,
robot_localization::ToLL::Response& response)
{
if (!transform_good_)
{
ROS_ERROR("No transform available (yet)");
return false;
}
tf2::Vector3 point;
tf2::fromMsg(request.map_point, point);
mapToLL(point, response.ll_point.latitude, response.ll_point.longitude, response.ll_point.altitude);
return true;
}
bool NavSatTransform::fromLLCallback(robot_localization::FromLL::Request& request,
robot_localization::FromLL::Response& response)
{
double altitude = request.ll_point.altitude;
double longitude = request.ll_point.longitude;
double latitude = request.ll_point.latitude;
tf2::Transform cartesian_pose;
double cartesian_x;
double cartesian_y;
double cartesian_z;
if (use_local_cartesian_)
{
gps_local_cartesian_.Forward(latitude, longitude, altitude, cartesian_x, cartesian_y, cartesian_z);
}
else
{
int zone_tmp;
bool nortp_tmp;
try
{
GeographicLib::UTMUPS::Forward(latitude, longitude, zone_tmp, nortp_tmp, cartesian_x, cartesian_y, utm_zone_);
}
catch (const GeographicLib::GeographicErr& e)
{
ROS_ERROR_STREAM_THROTTLE(1.0, e.what());
return false;
}
}
cartesian_pose.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, altitude));
nav_msgs::Odometry gps_odom;
if (!transform_good_)
{
ROS_ERROR("No transform available (yet)");
return false;
}
response.map_point = cartesianToMap(cartesian_pose).pose.pose.position;
return true;
}
bool NavSatTransform::setUTMZoneCallback(robot_localization::SetUTMZone::Request& request,
robot_localization::SetUTMZone::Response& response)
{
double x_unused;
double y_unused;
int prec_unused;
GeographicLib::MGRS::Reverse(request.utm_zone, utm_zone_, northp_, x_unused, y_unused, prec_unused, true);
ROS_INFO("UTM zone set to %d %s", utm_zone_, northp_ ? "north" : "south");
return true;
}
nav_msgs::Odometry NavSatTransform::cartesianToMap(const tf2::Transform& cartesian_pose) const
{
nav_msgs::Odometry gps_odom{};
tf2::Transform transformed_cartesian_gps{};
transformed_cartesian_gps.mult(cartesian_world_transform_, cartesian_pose);
transformed_cartesian_gps.setRotation(tf2::Quaternion::getIdentity());
// Set header information stamp because we would like to know the robot's position at that timestamp
gps_odom.header.frame_id = world_frame_id_;
gps_odom.header.stamp = gps_update_time_;
// Now fill out the message. Set the orientation to the identity.
tf2::toMsg(transformed_cartesian_gps, gps_odom.pose.pose);
gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z);
return gps_odom;
}
void NavSatTransform::mapToLL(const tf2::Vector3& point, double& latitude, double& longitude, double& altitude) const
{
tf2::Transform odom_as_cartesian{};
tf2::Transform pose{};
pose.setOrigin(point);
pose.setRotation(tf2::Quaternion::getIdentity());
odom_as_cartesian.mult(cartesian_world_trans_inverse_, pose);
odom_as_cartesian.setRotation(tf2::Quaternion::getIdentity());
if (use_local_cartesian_)
{
double altitude_tmp = 0.0;
gps_local_cartesian_.Reverse(odom_as_cartesian.getOrigin().getX(),
odom_as_cartesian.getOrigin().getY(),
0.0,
latitude,
longitude,
altitude_tmp);
altitude = odom_as_cartesian.getOrigin().getZ();
}
else
{
GeographicLib::UTMUPS::Reverse(utm_zone_,
northp_,
odom_as_cartesian.getOrigin().getX(),
odom_as_cartesian.getOrigin().getY(),
latitude,
longitude);
altitude = odom_as_cartesian.getOrigin().getZ();
}
}
void NavSatTransform::getRobotOriginCartesianPose(const tf2::Transform &gps_cartesian_pose,
tf2::Transform &robot_cartesian_pose,
const ros::Time &transform_time)
{
robot_cartesian_pose.setIdentity();
// Get linear offset from origin for the GPS
tf2::Transform offset;
bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
base_link_frame_id_,
gps_frame_id_,
transform_time,
ros::Duration(transform_timeout_),
offset,
tf_silent_failure_);
if (can_transform)
{
// Get the orientation we'll use for our Cartesian->world transform
tf2::Quaternion cartesian_orientation = transform_orientation_;
tf2::Matrix3x3 mat(cartesian_orientation);
// Add the offsets
double roll;
double pitch;
double yaw;
mat.getRPY(roll, pitch, yaw);
yaw += (magnetic_declination_ + yaw_offset_ + utm_meridian_convergence_);
cartesian_orientation.setRPY(roll, pitch, yaw);
// Rotate the GPS linear offset by the orientation
// Zero out the orientation, because the GPS orientation is meaningless, and if it's non-zero, it will make the
// the computation of robot_cartesian_pose erroneous.
offset.setOrigin(tf2::quatRotate(cartesian_orientation, offset.getOrigin()));
offset.setRotation(tf2::Quaternion::getIdentity());
// Update the initial pose
robot_cartesian_pose = offset.inverse() * gps_cartesian_pose;
}
else
{
if (gps_frame_id_ != "")
{
ROS_WARN_STREAM_ONCE("Unable to obtain " << base_link_frame_id_ << "->" << gps_frame_id_ <<
" transform. Will assume navsat device is mounted at robot's origin");
}
robot_cartesian_pose = gps_cartesian_pose;
}
}
void NavSatTransform::getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose,
tf2::Transform &robot_odom_pose,
const ros::Time &transform_time)
{
robot_odom_pose.setIdentity();
// Remove the offset from base_link
tf2::Transform gps_offset_rotated;
bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
base_link_frame_id_,
gps_frame_id_,
transform_time,
transform_timeout_,
gps_offset_rotated,
tf_silent_failure_);
if (can_transform)
{
tf2::Transform robot_orientation;
can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
world_frame_id_,
base_link_frame_id_,
transform_time,
transform_timeout_,
robot_orientation,
tf_silent_failure_);
if (can_transform)
{
// Zero out rotation because we don't care about the orientation of the
// GPS receiver relative to base_link
gps_offset_rotated.setOrigin(tf2::quatRotate(robot_orientation.getRotation(), gps_offset_rotated.getOrigin()));
gps_offset_rotated.setRotation(tf2::Quaternion::getIdentity());
robot_odom_pose = gps_offset_rotated.inverse() * gps_odom_pose;
}
else
{
ROS_WARN_STREAM_THROTTLE(5.0, "Could not obtain " << world_frame_id_ << "->" << base_link_frame_id_ <<
" transform. Will not remove offset of navsat device from robot's origin.");
}
}
else
{
ROS_WARN_STREAM_THROTTLE(5.0, "Could not obtain " << base_link_frame_id_ << "->" << gps_frame_id_ <<
" transform. Will not remove offset of navsat device from robot's origin.");
}
}
void NavSatTransform::gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg)
{
gps_frame_id_ = msg->header.frame_id;
if (gps_frame_id_.empty())
{
ROS_WARN_STREAM_ONCE("NavSatFix message has empty frame_id. Will assume navsat device is mounted at robot's "
"origin.");
}
// Make sure the GPS data is usable
bool good_gps = (msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX &&
!std::isnan(msg->altitude) &&
!std::isnan(msg->latitude) &&
!std::isnan(msg->longitude));
if (good_gps)
{
// If we haven't computed the transform yet, then
// store this message as the initial GPS data to use
if (!transform_good_ && !use_manual_datum_)
{
setTransformGps(msg);
}
double cartesian_x = 0.0;
double cartesian_y = 0.0;
double cartesian_z = 0.0;
if (use_local_cartesian_)
{
gps_local_cartesian_.Forward(msg->latitude, msg->longitude, msg->altitude,
cartesian_x, cartesian_y, cartesian_z);
}
else
{
// Transform to UTM using the fixed utm_zone_
int zone_tmp;
bool northp_tmp;
try
{
GeographicLib::UTMUPS::Forward(msg->latitude, msg->longitude,
zone_tmp, northp_tmp, cartesian_x, cartesian_y, utm_zone_);
}
catch (const GeographicLib::GeographicErr& e)
{
ROS_ERROR_STREAM_THROTTLE(1.0, e.what());
return;
}
}
latest_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude));
latest_cartesian_covariance_.setZero();
// Copy the measurement's covariance matrix so that we can rotate it later
for (size_t i = 0; i < POSITION_SIZE; i++)
{
for (size_t j = 0; j < POSITION_SIZE; j++)
{
latest_cartesian_covariance_(i, j) = msg->position_covariance[POSITION_SIZE * i + j];
}
}
gps_update_time_ = msg->header.stamp;
gps_updated_ = true;
}
}
void NavSatTransform::imuCallback(const sensor_msgs::ImuConstPtr& msg)
{
// We need the baseLinkFrameId_ from the odometry message, so
// we need to wait until we receive it.
if (has_transform_odom_)
{
/* This method only gets called if we don't yet have the
* IMU data (the subscriber gets shut down once we compute
* the transform), so we can assumed that every IMU message
* that comes here is meant to be used for that purpose. */
tf2::fromMsg(msg->orientation, transform_orientation_);
// Correct for the IMU's orientation w.r.t. base_link
tf2::Transform target_frame_trans;
bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_,
base_link_frame_id_,
msg->header.frame_id,
msg->header.stamp,
transform_timeout_,
target_frame_trans,
tf_silent_failure_);
if (can_transform)
{
double roll_offset = 0;
double pitch_offset = 0;
double yaw_offset = 0;
double roll = 0;
double pitch = 0;
double yaw = 0;
RosFilterUtilities::quatToRPY(target_frame_trans.getRotation(), roll_offset, pitch_offset, yaw_offset);
RosFilterUtilities::quatToRPY(transform_orientation_, roll, pitch, yaw);
ROS_DEBUG_STREAM("Initial orientation is " << transform_orientation_);
// Apply the offset (making sure to bound them), and throw them in a vector
tf2::Vector3 rpy_angles(FilterUtilities::clampRotation(roll - roll_offset),
FilterUtilities::clampRotation(pitch - pitch_offset),
FilterUtilities::clampRotation(yaw - yaw_offset));
// Now we need to rotate the roll and pitch by the yaw offset value.
// Imagine a case where an IMU is mounted facing sideways. In that case
// pitch for the IMU's world frame is roll for the robot.
tf2::Matrix3x3 mat;
mat.setRPY(0.0, 0.0, yaw_offset);
rpy_angles = mat * rpy_angles;
transform_orientation_.setRPY(rpy_angles.getX(), rpy_angles.getY(), rpy_angles.getZ());
ROS_DEBUG_STREAM("Initial corrected orientation roll, pitch, yaw is (" <<
rpy_angles.getX() << ", " << rpy_angles.getY() << ", " << rpy_angles.getZ() << ")");
has_transform_imu_ = true;
}
}
}
void NavSatTransform::odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
world_frame_id_ = msg->header.frame_id;
base_link_frame_id_ = msg->child_frame_id;
if (!transform_good_ && !use_manual_datum_)
{
setTransformOdometry(msg);
}
tf2::fromMsg(msg->pose.pose, latest_world_pose_);
latest_odom_covariance_.setZero();
for (size_t row = 0; row < POSE_SIZE; ++row)
{
for (size_t col = 0; col < POSE_SIZE; ++col)
{
latest_odom_covariance_(row, col) = msg->pose.covariance[row * POSE_SIZE + col];
}
}
odom_update_time_ = msg->header.stamp;
odom_updated_ = true;
}
bool NavSatTransform::prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps)
{
bool new_data = false;
if (transform_good_ && odom_updated_)
{
mapToLL(latest_world_pose_.getOrigin(), filtered_gps.latitude, filtered_gps.longitude, filtered_gps.altitude);
// Rotate the covariance as well
tf2::Matrix3x3 rot(cartesian_world_trans_inverse_.getRotation());
Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE);
rot_6d.setIdentity();
for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
{
rot_6d(rInd, 0) = rot.getRow(rInd).getX();
rot_6d(rInd, 1) = rot.getRow(rInd).getY();
rot_6d(rInd, 2) = rot.getRow(rInd).getZ();
rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
}
// Rotate the covariance
latest_odom_covariance_ = rot_6d * latest_odom_covariance_.eval() * rot_6d.transpose();
// Copy the measurement's covariance matrix back
for (size_t i = 0; i < POSITION_SIZE; i++)
{
for (size_t j = 0; j < POSITION_SIZE; j++)
{
filtered_gps.position_covariance[POSITION_SIZE * i + j] = latest_odom_covariance_(i, j);
}
}
filtered_gps.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
filtered_gps.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
filtered_gps.header.frame_id = base_link_frame_id_;
filtered_gps.header.stamp = odom_update_time_;
// Mark this GPS as used
odom_updated_ = false;
new_data = true;
}
return new_data;
}
bool NavSatTransform::prepareGpsOdometry(nav_msgs::Odometry &gps_odom)
{
bool new_data = false;
if (transform_good_ && gps_updated_ && odom_updated_)
{
gps_odom = cartesianToMap(latest_cartesian_pose_);
tf2::Transform transformed_cartesian_gps;
tf2::fromMsg(gps_odom.pose.pose, transformed_cartesian_gps);
// Want the pose of the vehicle origin, not the GPS
tf2::Transform transformed_cartesian_robot;
getRobotOriginWorldPose(transformed_cartesian_gps, transformed_cartesian_robot, gps_odom.header.stamp);
// Rotate the covariance as well
tf2::Matrix3x3 rot(cartesian_world_transform_.getRotation());
Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE);
rot_6d.setIdentity();
for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
{
rot_6d(rInd, 0) = rot.getRow(rInd).getX();
rot_6d(rInd, 1) = rot.getRow(rInd).getY();
rot_6d(rInd, 2) = rot.getRow(rInd).getZ();
rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
}
// Rotate the covariance
latest_cartesian_covariance_ = rot_6d * latest_cartesian_covariance_.eval() * rot_6d.transpose();
// Now fill out the message. Set the orientation to the identity.
tf2::toMsg(transformed_cartesian_robot, gps_odom.pose.pose);
gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z);
// Copy the measurement's covariance matrix so that we can rotate it later
for (size_t i = 0; i < POSE_SIZE; i++)
{
for (size_t j = 0; j < POSE_SIZE; j++)
{
gps_odom.pose.covariance[POSE_SIZE * i + j] = latest_cartesian_covariance_(i, j);
}
}
// Mark this GPS as used
gps_updated_ = false;
new_data = true;
}
return new_data;
}
void NavSatTransform::setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg)
{
double cartesian_x = 0;
double cartesian_y = 0;
double cartesian_z = 0;
if (use_local_cartesian_)
{
const double hae_altitude = 0.0;
gps_local_cartesian_.Reset(msg->latitude, msg->longitude, hae_altitude);
gps_local_cartesian_.Forward(msg->latitude, msg->longitude, msg->altitude, cartesian_x, cartesian_y, cartesian_z);
// UTM meridian convergence is not meaningful when using local cartesian, so set it to 0.0
utm_meridian_convergence_ = 0.0;
}
else
{
double k_tmp;
double utm_meridian_convergence_degrees;
GeographicLib::UTMUPS::Forward(msg->latitude, msg->longitude, utm_zone_, northp_,
cartesian_x, cartesian_y, utm_meridian_convergence_degrees, k_tmp);
utm_meridian_convergence_ = utm_meridian_convergence_degrees * NavsatConversions::RADIANS_PER_DEGREE;
}
ROS_INFO_STREAM("Datum (latitude, longitude, altitude) is (" << std::fixed << msg->latitude << ", " <<
msg->longitude << ", " << msg->altitude << ")");
ROS_INFO_STREAM("Datum " << ((use_local_cartesian_)? "Local Cartesian" : "UTM") <<
" coordinate is (" << std::fixed << cartesian_x << ", " << cartesian_y << ") zone " << utm_zone_);
transform_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude));
transform_cartesian_pose_.setRotation(tf2::Quaternion::getIdentity());
has_transform_gps_ = true;
}
void NavSatTransform::setTransformOdometry(const nav_msgs::OdometryConstPtr& msg)
{
tf2::fromMsg(msg->pose.pose, transform_world_pose_);
has_transform_odom_ = true;
ROS_INFO_STREAM_ONCE("Initial odometry pose is " << transform_world_pose_);
// Users can optionally use the (potentially fused) heading from
// the odometry source, which may have multiple fused sources of
// heading data, and so would act as a better heading for the
// Cartesian->world_frame transform.
if (!transform_good_ && use_odometry_yaw_ && !use_manual_datum_)
{
sensor_msgs::Imu *imu = new sensor_msgs::Imu();
imu->orientation = msg->pose.pose.orientation;
imu->header.frame_id = msg->child_frame_id;
imu->header.stamp = msg->header.stamp;
sensor_msgs::ImuConstPtr imuPtr(imu);
imuCallback(imuPtr);
}
}
} // namespace RobotLocalization

51
Localizations/Packages/robot_localization/src/navsat_transform_node.cpp Normal file
View File

@@ -0,0 +1,51 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/navsat_transform.h"
#include <ros/ros.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "navsat_transform_node");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
RobotLocalization::NavSatTransform trans(nh, nh_priv);
ros::spin();
return EXIT_SUCCESS;
}

63
Localizations/Packages/robot_localization/src/navsat_transform_nodelet.cpp Normal file
View File

@@ -0,0 +1,63 @@
/*
* Copyright (c) 2017 Simon Gene Gottlieb
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/navsat_transform.h"
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <memory>
namespace RobotLocalization
{
class NavSatTransformNodelet : public nodelet::Nodelet
{
private:
std::unique_ptr<RobotLocalization::NavSatTransform> trans;
public:
virtual void onInit()
{
NODELET_DEBUG("Initializing nodelet...");
ros::NodeHandle nh = getNodeHandle();
ros::NodeHandle nh_priv = getPrivateNodeHandle();
trans = std::make_unique<RobotLocalization::NavSatTransform>(nh, nh_priv);
}
};
} // namespace RobotLocalization
PLUGINLIB_EXPORT_CLASS(RobotLocalization::NavSatTransformNodelet, nodelet::Nodelet);

212
Localizations/Packages/robot_localization/src/robot_localization_estimator.cpp Normal file
View File

@@ -0,0 +1,212 @@
/*
* Copyright (c) 2016, TNO IVS Helmond.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/robot_localization_estimator.h"
#include "robot_localization/ekf.h"
#include "robot_localization/ukf.h"
#include <vector>
namespace RobotLocalization
{
RobotLocalizationEstimator::RobotLocalizationEstimator(unsigned int buffer_capacity,
FilterType filter_type,
const Eigen::MatrixXd& process_noise_covariance,
const std::vector<double>& filter_args)
{
state_buffer_.set_capacity(buffer_capacity);
// Set up the filter that is used for predictions
if ( filter_type == FilterTypes::EKF )
{
filter_ = new Ekf;
}
else if ( filter_type == FilterTypes::UKF )
{
filter_ = new Ukf(filter_args);
}
filter_->setProcessNoiseCovariance(process_noise_covariance);
}
RobotLocalizationEstimator::~RobotLocalizationEstimator()
{
delete filter_;
}
void RobotLocalizationEstimator::setState(const EstimatorState& state)
{
// If newly received state is newer than any in the buffer, push back
if ( state_buffer_.empty() || state.time_stamp > state_buffer_.back().time_stamp )
{
state_buffer_.push_back(state);
}
// If it is older, put it in the right position
else
{
for ( boost::circular_buffer<EstimatorState>::iterator it = state_buffer_.begin(); it != state_buffer_.end(); ++it )
{
if ( state.time_stamp < it->time_stamp )
{
state_buffer_.insert(it, state);
return;
}
}
}
}
EstimatorResult RobotLocalizationEstimator::getState(const double time,
EstimatorState& state) const
{
// If there's nothing in the buffer, there's nothing to give.
if ( state_buffer_.size() == 0 )
{
return EstimatorResults::EmptyBuffer;
}
// Set state to the most recent one for now
state = state_buffer_.back();
// Go through buffer from new to old
EstimatorState last_state_before_time = state_buffer_.front();
EstimatorState next_state_after_time = state_buffer_.back();
bool previous_state_found = false;
bool next_state_found = false;
for (boost::circular_buffer<EstimatorState>::const_reverse_iterator it = state_buffer_.rbegin();
it != state_buffer_.rend(); ++it)
{
/* If the time stamp of the current state from the buffer is
* older than the requested time, store it as the last state
* before the requested time. If it is younger, save it as the
* next one after, and go on to find the last one before.
*/
if ( it->time_stamp == time )
{
state = *it;
return EstimatorResults::Exact;
}
else if ( it->time_stamp <= time )
{
last_state_before_time = *it;
previous_state_found = true;
break;
}
else
{
next_state_after_time = *it;
next_state_found = true;
}
}
// If we found a previous state and a next state, we can do interpolation
if ( previous_state_found && next_state_found )
{
interpolate(last_state_before_time, next_state_after_time, time, state);
return EstimatorResults::Interpolation;
}
// If only a previous state is found, we can do extrapolation into the future
else if ( previous_state_found )
{
extrapolate(last_state_before_time, time, state);
return EstimatorResults::ExtrapolationIntoFuture;
}
// If only a next state is found, we'll have to extrapolate into the past.
else if ( next_state_found )
{
extrapolate(next_state_after_time, time, state);
return EstimatorResults::ExtrapolationIntoPast;
}
else
{
return EstimatorResults::Failed;
}
}
void RobotLocalizationEstimator::setBufferCapacity(const int capacity)
{
state_buffer_.set_capacity(capacity);
}
void RobotLocalizationEstimator::clearBuffer()
{
state_buffer_.clear();
}
unsigned int RobotLocalizationEstimator::getBufferCapacity() const
{
return state_buffer_.capacity();
}
unsigned int RobotLocalizationEstimator::getSize() const
{
return state_buffer_.size();
}
void RobotLocalizationEstimator::extrapolate(const EstimatorState& boundary_state,
const double requested_time,
EstimatorState& state_at_req_time) const
{
// Set up the filter with the boundary state
filter_->setState(boundary_state.state);
filter_->setEstimateErrorCovariance(boundary_state.covariance);
// Calculate how much time we need to extrapolate into the future
double delta = requested_time - boundary_state.time_stamp;
// Use the filter to predict
filter_->predict(boundary_state.time_stamp, delta);
state_at_req_time.time_stamp = requested_time;
state_at_req_time.state = filter_->getState();
state_at_req_time.covariance = filter_->getEstimateErrorCovariance();
return;
}
void RobotLocalizationEstimator::interpolate(const EstimatorState& given_state_1,
const EstimatorState& given_state_2,
const double requested_time,
EstimatorState& state_at_req_time) const
{
/*
* TODO: Right now, we only extrapolate from the last known state before the requested time. But as the state after
* the requested time is also known, we may want to perform interpolation between states.
*/
extrapolate(given_state_1, requested_time, state_at_req_time);
return;
}
} // namespace RobotLocalization

98
Localizations/Packages/robot_localization/src/robot_localization_listener_node.cpp Normal file
View File

@@ -0,0 +1,98 @@
/*
* Copyright (c) 2016, TNO IVS Helmond.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_robot_localization_listener.h"
#include "robot_localization/GetState.h"
#include <string>
namespace RobotLocalization
{
class RobotLocalizationListenerNode
{
public:
RobotLocalizationListenerNode()
{
service_ = n_.advertiseService("get_state", &RobotLocalizationListenerNode::getStateCallback, this);
}
std::string getService()
{
return service_.getService();
}
private:
RosRobotLocalizationListener rll_;
ros::NodeHandle n_;
ros::ServiceServer service_;
bool getStateCallback(robot_localization::GetState::Request &req,
robot_localization::GetState::Response &res)
{
Eigen::VectorXd state(STATE_SIZE);
Eigen::MatrixXd covariance(STATE_SIZE, STATE_SIZE);
if ( !rll_.getState(req.time_stamp, req.frame_id, state, covariance) )
{
ROS_ERROR("Robot Localization Listener Node: Listener instance returned false at getState call.");
return false;
}
for (size_t i = 0; i < STATE_SIZE; i++)
{
res.state[i] = (*(state.data() + i));
}
for (size_t i = 0; i < STATE_SIZE * STATE_SIZE; i++)
{
res.covariance[i] = (*(covariance.data() + i));
}
ROS_DEBUG("Robot Localization Listener Node: Listener responded with state and covariance at the requested time.");
return true;
}
};
} // namespace RobotLocalization
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot_localization_listener_node");
RobotLocalization::RobotLocalizationListenerNode rlln;
ROS_INFO_STREAM("Robot Localization Listener Node: Ready to handle GetState requests at " << rlln.getService());
ros::spin();
return 0;
}

3361
Localizations/Packages/robot_localization/src/ros_filter.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

195
Localizations/Packages/robot_localization/src/ros_filter_utilities.cpp Normal file
View File

@@ -0,0 +1,195 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_utilities.h"
#include "robot_localization/filter_common.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <ros/console.h>
#include <string>
#include <vector>
std::ostream& operator<<(std::ostream& os, const tf2::Vector3 &vec)
{
os << "(" << std::setprecision(20) << vec.getX() << " " << vec.getY() << " " << vec.getZ() << ")\n";
return os;
}
std::ostream& operator<<(std::ostream& os, const tf2::Quaternion &quat)
{
double roll, pitch, yaw;
tf2::Matrix3x3 orTmp(quat);
orTmp.getRPY(roll, pitch, yaw);
os << "(" << std::setprecision(20) << roll << ", " << pitch << ", " << yaw << ")\n";
return os;
}
std::ostream& operator<<(std::ostream& os, const tf2::Transform &trans)
{
os << "Origin: " << trans.getOrigin() <<
"Rotation (RPY): " << trans.getRotation();
return os;
}
std::ostream& operator<<(std::ostream& os, const std::vector<double> &vec)
{
os << "(" << std::setprecision(20);
for (size_t i = 0; i < vec.size(); ++i)
{
os << vec[i] << " ";
}
os << ")\n";
return os;
}
namespace RobotLocalization
{
namespace RosFilterUtilities
{
double getYaw(const tf2::Quaternion quat)
{
tf2::Matrix3x3 mat(quat);
double dummy;
double yaw;
mat.getRPY(dummy, dummy, yaw);
return yaw;
}
bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
const std::string &targetFrame,
const std::string &sourceFrame,
const ros::Time &time,
const ros::Duration &timeout,
tf2::Transform &targetFrameTrans,
const bool silent)
{
bool retVal = true;
// First try to transform the data at the requested time
try
{
tf2::fromMsg(buffer.lookupTransform(targetFrame, sourceFrame, time, timeout).transform,
targetFrameTrans);
}
catch (tf2::TransformException &ex)
{
// The issue might be that the transforms that are available are not close
// enough temporally to be used. In that case, just use the latest available
// transform and warn the user.
try
{
tf2::fromMsg(buffer.lookupTransform(targetFrame, sourceFrame, ros::Time(0)).transform,
targetFrameTrans);
if (!silent)
{
ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << sourceFrame << " to " << targetFrame <<
" was unavailable for the time requested. Using latest instead.\n");
}
}
catch(tf2::TransformException &ex)
{
if (!silent)
{
ROS_WARN_STREAM_THROTTLE(2.0, "Could not obtain transform from " << sourceFrame <<
" to " << targetFrame << ". Error was " << ex.what() << "\n");
}
retVal = false;
}
}
// Transforming from a frame id to itself can fail when the tf tree isn't
// being broadcast (e.g., for some bag files). This is the only failure that
// would throw an exception, so check for this situation before giving up.
if (!retVal)
{
if (targetFrame == sourceFrame)
{
targetFrameTrans.setIdentity();
retVal = true;
}
}
return retVal;
}
bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
const std::string &targetFrame,
const std::string &sourceFrame,
const ros::Time &time,
tf2::Transform &targetFrameTrans,
const bool silent)
{
return lookupTransformSafe(buffer, targetFrame, sourceFrame, time, ros::Duration(0), targetFrameTrans, silent);
}
void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw)
{
tf2::Matrix3x3 orTmp(quat);
orTmp.getRPY(roll, pitch, yaw);
}
void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF)
{
stateTF.setOrigin(tf2::Vector3(state(StateMemberX),
state(StateMemberY),
state(StateMemberZ)));
tf2::Quaternion quat;
quat.setRPY(state(StateMemberRoll),
state(StateMemberPitch),
state(StateMemberYaw));
stateTF.setRotation(quat);
}
void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state)
{
state(StateMemberX) = stateTF.getOrigin().getX();
state(StateMemberY) = stateTF.getOrigin().getY();
state(StateMemberZ) = stateTF.getOrigin().getZ();
quatToRPY(stateTF.getRotation(), state(StateMemberRoll), state(StateMemberPitch), state(StateMemberYaw));
}
} // namespace RosFilterUtilities
} // namespace RobotLocalization

539
Localizations/Packages/robot_localization/src/ros_robot_localization_listener.cpp Normal file
View File

@@ -0,0 +1,539 @@
/*
* Copyright (c) 2016, TNO IVS Helmond.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_robot_localization_listener.h"
#include "robot_localization/ros_filter_utilities.h"
#include <map>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <yaml-cpp/yaml.h>
#include <eigen_conversions/eigen_msg.h>
#include <XmlRpcException.h>
namespace RobotLocalization
{
FilterType filterTypeFromString(const std::string& filter_type_str)
{
if ( filter_type_str == "ekf" )
{
return FilterTypes::EKF;
}
else if ( filter_type_str == "ukf" )
{
return FilterTypes::UKF;
}
else
{
return FilterTypes::NotDefined;
}
}
RosRobotLocalizationListener::RosRobotLocalizationListener():
nh_p_("robot_localization"),
odom_sub_(nh_, "odometry/filtered", 1),
accel_sub_(nh_, "accel/filtered", 1),
sync_(odom_sub_, accel_sub_, 10),
base_frame_id_(""),
world_frame_id_(""),
tf_listener_(tf_buffer_)
{
int buffer_size;
nh_p_.param("buffer_size", buffer_size, 10);
std::string param_ns;
nh_p_.param("parameter_namespace", param_ns, nh_p_.getNamespace());
ros::NodeHandle nh_param(param_ns);
std::string filter_type_str;
nh_param.param("filter_type", filter_type_str, std::string("ekf"));
FilterType filter_type = filterTypeFromString(filter_type_str);
if ( filter_type == FilterTypes::NotDefined )
{
ROS_ERROR("RosRobotLocalizationListener: Parameter filter_type invalid");
return;
}
// Load up the process noise covariance (from the launch file/parameter server)
// TODO(reinzor): this code is copied from ros_filter. In a refactor, this could be moved to a function in
// ros_filter_utilities
Eigen::MatrixXd process_noise_covariance(STATE_SIZE, STATE_SIZE);
process_noise_covariance.setZero();
XmlRpc::XmlRpcValue process_noise_covar_config;
if (!nh_param.hasParam("process_noise_covariance"))
{
ROS_FATAL_STREAM("Process noise covariance not found in the robot localization listener config (namespace " <<
nh_param.getNamespace() << ")! Use the ~parameter_namespace to specify the parameter namespace.");
}
else
{
try
{
nh_param.getParam("process_noise_covariance", process_noise_covar_config);
ROS_ASSERT(process_noise_covar_config.getType() == XmlRpc::XmlRpcValue::TypeArray);
int mat_size = process_noise_covariance.rows();
for (int i = 0; i < mat_size; i++)
{
for (int j = 0; j < mat_size; j++)
{
try
{
// These matrices can cause problems if all the types
// aren't specified with decimal points. Handle that
// using string streams.
std::ostringstream ostr;
process_noise_covar_config[mat_size * i + j].write(ostr);
std::istringstream istr(ostr.str());
istr >> process_noise_covariance(i, j);
}
catch(XmlRpc::XmlRpcException &e)
{
throw e;
}
catch(...)
{
throw;
}
}
}
ROS_DEBUG_STREAM("Process noise covariance is:\n" << process_noise_covariance << "\n");
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading robot localization listener config: " <<
e.getMessage() <<
" for process_noise_covariance (type: " <<
process_noise_covar_config.getType() << ")");
}
}
std::vector<double> filter_args;
nh_param.param("filter_args", filter_args, std::vector<double>());
estimator_ = new RobotLocalizationEstimator(buffer_size, filter_type, process_noise_covariance, filter_args);
sync_.registerCallback(&RosRobotLocalizationListener::odomAndAccelCallback, this);
ROS_INFO_STREAM("Ros Robot Localization Listener: Listening to topics " <<
odom_sub_.getTopic() << " and " << accel_sub_.getTopic());
// Wait until the base and world frames are set by the incoming messages
while (ros::ok() && base_frame_id_.empty())
{
ros::spinOnce();
ROS_INFO_STREAM_THROTTLE(1.0, "Ros Robot Localization Listener: Waiting for incoming messages on topics " <<
odom_sub_.getTopic() << " and " << accel_sub_.getTopic());
ros::Duration(0.1).sleep();
}
}
RosRobotLocalizationListener::~RosRobotLocalizationListener()
{
delete estimator_;
}
void RosRobotLocalizationListener::odomAndAccelCallback(const nav_msgs::Odometry& odom,
const geometry_msgs::AccelWithCovarianceStamped& accel)
{
// Instantiate a state that can be added to the robot localization estimator
EstimatorState state;
// Set its time stamp and the state received from the messages
state.time_stamp = odom.header.stamp.toSec();
// Get the base frame id from the odom message
if ( base_frame_id_.empty() )
base_frame_id_ = odom.child_frame_id;
// Get the world frame id from the odom message
if ( world_frame_id_.empty() )
world_frame_id_ = odom.header.frame_id;
// Pose: Position
state.state(StateMemberX) = odom.pose.pose.position.x;
state.state(StateMemberY) = odom.pose.pose.position.y;
state.state(StateMemberZ) = odom.pose.pose.position.z;
// Pose: Orientation
tf2::Quaternion orientation_quat;
tf2::fromMsg(odom.pose.pose.orientation, orientation_quat);
double roll, pitch, yaw;
RosFilterUtilities::quatToRPY(orientation_quat, roll, pitch, yaw);
state.state(StateMemberRoll) = roll;
state.state(StateMemberPitch) = pitch;
state.state(StateMemberYaw) = yaw;
// Pose: Covariance
for ( unsigned int i = 0; i < POSE_SIZE; i++ )
{
for ( unsigned int j = 0; j < POSE_SIZE; j++ )
{
state.covariance(POSITION_OFFSET + i, POSITION_OFFSET + j) = odom.pose.covariance[i*POSE_SIZE + j];
}
}
// Velocity: Linear
state.state(StateMemberVx) = odom.twist.twist.linear.x;
state.state(StateMemberVy) = odom.twist.twist.linear.y;
state.state(StateMemberVz) = odom.twist.twist.linear.z;
// Velocity: Angular
state.state(StateMemberVroll) = odom.twist.twist.angular.x;
state.state(StateMemberVpitch) = odom.twist.twist.angular.y;
state.state(StateMemberVyaw) = odom.twist.twist.angular.z;
// Velocity: Covariance
for ( unsigned int i = 0; i < TWIST_SIZE; i++ )
{
for ( unsigned int j = 0; j < TWIST_SIZE; j++ )
{
state.covariance(POSITION_V_OFFSET + i, POSITION_V_OFFSET + j) = odom.twist.covariance[i*TWIST_SIZE + j];
}
}
// Acceleration: Linear
state.state(StateMemberAx) = accel.accel.accel.linear.x;
state.state(StateMemberAy) = accel.accel.accel.linear.y;
state.state(StateMemberAz) = accel.accel.accel.linear.z;
// Acceleration: Angular is not available in state
// Acceleration: Covariance
for ( unsigned int i = 0; i < ACCELERATION_SIZE; i++ )
{
for ( unsigned int j = 0; j < ACCELERATION_SIZE; j++ )
{
state.covariance(POSITION_A_OFFSET + i, POSITION_A_OFFSET + j) = accel.accel.covariance[i*TWIST_SIZE + j];
}
}
// Add the state to the buffer, so that we can later interpolate between this and earlier states
estimator_->setState(state);
return;
}
bool findAncestorRecursiveYAML(YAML::Node& tree, const std::string& source_frame, const std::string& target_frame)
{
if ( source_frame == target_frame )
{
return true;
}
std::string parent_frame = tree[source_frame]["parent"].Scalar();
if ( parent_frame.empty() )
{
return false;
}
return findAncestorRecursiveYAML(tree, parent_frame, target_frame);
}
// Cache, assumption that the tree parent child order does not change over time
static std::map<std::string, std::vector<std::string> > ancestor_map;
static std::map<std::string, std::vector<std::string> > descendant_map;
bool findAncestor(const tf2_ros::Buffer& buffer, const std::string& source_frame, const std::string& target_frame)
{
// Check cache
const std::vector<std::string>& ancestors = ancestor_map[source_frame];
if (std::find(ancestors.begin(), ancestors.end(), target_frame) != ancestors.end())
{
return true;
}
const std::vector<std::string>& descendants = descendant_map[source_frame];
if (std::find(descendants.begin(), descendants.end(), target_frame) != descendants.end())
{
return false;
}
std::stringstream frames_stream(buffer.allFramesAsYAML());
YAML::Node frames_yaml = YAML::Load(frames_stream);
bool target_frame_is_ancestor = findAncestorRecursiveYAML(frames_yaml, source_frame, target_frame);
bool target_frame_is_descendant = findAncestorRecursiveYAML(frames_yaml, target_frame, source_frame);
// Caching
if (target_frame_is_ancestor)
{
ancestor_map[source_frame].push_back(target_frame);
}
if (target_frame_is_descendant)
{
descendant_map[source_frame].push_back(target_frame);
}
return target_frame_is_ancestor;
}
bool RosRobotLocalizationListener::getState(const double time,
const std::string& frame_id,
Eigen::VectorXd& state,
Eigen::MatrixXd& covariance,
std::string world_frame_id) const
{
EstimatorState estimator_state;
state.resize(STATE_SIZE);
state.setZero();
covariance.resize(STATE_SIZE, STATE_SIZE);
covariance.setZero();
if ( base_frame_id_.empty() || world_frame_id_.empty() )
{
if ( estimator_->getSize() == 0 )
{
ROS_WARN("Ros Robot Localization Listener: The base or world frame id is not set. "
"No odom/accel messages have come in.");
}
else
{
ROS_ERROR("Ros Robot Localization Listener: The base or world frame id is not set. "
"Are the child_frame_id and the header.frame_id in the odom messages set?");
}
return false;
}
if ( estimator_->getState(time, estimator_state) == EstimatorResults::ExtrapolationIntoPast )
{
ROS_WARN("Ros Robot Localization Listener: A state is requested at a time stamp older than the oldest in the "
"estimator buffer. The result is an extrapolation into the past. Maybe you should increase the buffer "
"size?");
}
// If no world_frame_id is specified, we will default to the world frame_id of the received odometry message
if (world_frame_id.empty())
{
world_frame_id = world_frame_id_;
}
if ( frame_id == base_frame_id_ && world_frame_id == world_frame_id_ )
{
// If the state of the base frame is requested and the world frame equals the world frame of the robot_localization
// estimator, we can simply return the state we got from the state estimator
state = estimator_state.state;
covariance = estimator_state.covariance;
return true;
}
// - - - - - - - - - - - - - - - - - -
// Get the transformation between the requested world frame and the world_frame of the estimator
// - - - - - - - - - - - - - - - - - -
Eigen::Affine3d world_pose_requested_frame;
// If the requested frame is the same as the tracker, set to identity
if (world_frame_id == world_frame_id_)
{
world_pose_requested_frame.setIdentity();
}
else
{
geometry_msgs::TransformStamped world_requested_to_world_transform;
try
{
// TODO(reinzor): magic number
world_requested_to_world_transform = tf_buffer_.lookupTransform(world_frame_id,
world_frame_id_,
ros::Time(time),
ros::Duration(0.1));
if ( findAncestor(tf_buffer_, world_frame_id, base_frame_id_) )
{
ROS_ERROR_STREAM("You are trying to get the state with respect to world frame " << world_frame_id <<
", but this frame is a child of robot base frame " << base_frame_id_ <<
", so this doesn't make sense.");
return false;
}
}
catch ( const tf2::TransformException &e )
{
ROS_WARN_STREAM("Ros Robot Localization Listener: Could not look up transform: " << e.what());
return false;
}
// Convert to pose
tf::transformMsgToEigen(world_requested_to_world_transform.transform, world_pose_requested_frame);
}
// - - - - - - - - - - - - - - - - - -
// Calculate the state of the requested frame from the state of the base frame.
// - - - - - - - - - - - - - - - - - -
// First get the transform from base to target
geometry_msgs::TransformStamped base_to_target_transform;
try
{
base_to_target_transform = tf_buffer_.lookupTransform(base_frame_id_,
frame_id,
ros::Time(time),
ros::Duration(0.1)); // TODO(reinzor): magic number
// Check that frame_id is a child of the base frame. If it is not, it does not make sense to request its state.
// Do this after tf lookup, so we know that there is a connection.
if ( !findAncestor(tf_buffer_, frame_id, base_frame_id_) )
{
ROS_ERROR_STREAM("You are trying to get the state of " << frame_id << ", but this frame is not a child of the "
"base frame: " << base_frame_id_ << ".");
return false;
}
}
catch ( const tf2::TransformException &e )
{
ROS_WARN_STREAM("Ros Robot Localization Listener: Could not look up transform: " << e.what());
return false;
}
// And convert it to an eigen Affine transformation
Eigen::Affine3d target_pose_base;
tf::transformMsgToEigen(base_to_target_transform.transform, target_pose_base);
// Then convert the base pose to an Eigen Affine transformation
Eigen::Vector3d base_position(estimator_state.state(StateMemberX),
estimator_state.state(StateMemberY),
estimator_state.state(StateMemberZ));
Eigen::AngleAxisd roll_angle(estimator_state.state(StateMemberRoll), Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitch_angle(estimator_state.state(StateMemberPitch), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yaw_angle(estimator_state.state(StateMemberYaw), Eigen::Vector3d::UnitZ());
Eigen::Quaterniond base_orientation(yaw_angle * pitch_angle * roll_angle);
Eigen::Affine3d base_pose(Eigen::Translation3d(base_position) * base_orientation);
// Now we can calculate the transform from odom to the requested frame (target)...
Eigen::Affine3d target_pose_odom = world_pose_requested_frame * base_pose * target_pose_base;
// ... and put it in the output state
state(StateMemberX) = target_pose_odom.translation().x();
state(StateMemberY) = target_pose_odom.translation().y();
state(StateMemberZ) = target_pose_odom.translation().z();
Eigen::Vector3d ypr = target_pose_odom.rotation().eulerAngles(2, 1, 0);
state(StateMemberRoll) = ypr[2];
state(StateMemberPitch) = ypr[1];
state(StateMemberYaw) = ypr[0];
// Now let's calculate the twist of the target frame
// First get the base's twist
Twist base_velocity;
Twist target_velocity_base;
base_velocity.linear = Eigen::Vector3d(estimator_state.state(StateMemberVx),
estimator_state.state(StateMemberVy),
estimator_state.state(StateMemberVz));
base_velocity.angular = Eigen::Vector3d(estimator_state.state(StateMemberVroll),
estimator_state.state(StateMemberVpitch),
estimator_state.state(StateMemberVyaw));
// Then calculate the target frame's twist as a result of the base's twist.
/*
* We first calculate the coordinates of the velocity vectors (linear and angular) in the base frame. We have to keep
* in mind that a rotation of the base frame, together with the translational offset of the target frame from the base
* frame, induces a translational velocity of the target frame.
*/
target_velocity_base.linear = base_velocity.linear + base_velocity.angular.cross(target_pose_base.translation());
target_velocity_base.angular = base_velocity.angular;
// Now we can transform that to the target frame
Twist target_velocity;
target_velocity.linear = target_pose_base.rotation().transpose() * target_velocity_base.linear;
target_velocity.angular = target_pose_base.rotation().transpose() * target_velocity_base.angular;
state(StateMemberVx) = target_velocity.linear(0);
state(StateMemberVy) = target_velocity.linear(1);
state(StateMemberVz) = target_velocity.linear(2);
state(StateMemberVroll) = target_velocity.angular(0);
state(StateMemberVpitch) = target_velocity.angular(1);
state(StateMemberVyaw) = target_velocity.angular(2);
// Rotate the covariance as well
Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE);
rot_6d.setIdentity();
rot_6d.block<POSITION_SIZE, POSITION_SIZE>(POSITION_OFFSET, POSITION_OFFSET) = target_pose_base.rotation();
rot_6d.block<ORIENTATION_SIZE, ORIENTATION_SIZE>(ORIENTATION_OFFSET, ORIENTATION_OFFSET) =
target_pose_base.rotation();
// Rotate the covariance
covariance.block<POSE_SIZE, POSE_SIZE>(POSITION_OFFSET, POSITION_OFFSET) =
rot_6d * estimator_state.covariance.block<POSE_SIZE, POSE_SIZE>(POSITION_OFFSET, POSITION_OFFSET) *
rot_6d.transpose();
return true;
}
bool RosRobotLocalizationListener::getState(const ros::Time& ros_time, const std::string& frame_id,
Eigen::VectorXd& state, Eigen::MatrixXd& covariance,
const std::string& world_frame_id) const
{
double time;
if ( ros_time.isZero() )
{
ROS_INFO("Ros Robot Localization Listener: State requested at time = zero, returning state at current time");
time = ros::Time::now().toSec();
}
else
{
time = ros_time.toSec();
}
return getState(time, frame_id, state, covariance, world_frame_id);
}
const std::string& RosRobotLocalizationListener::getBaseFrameId() const
{
return base_frame_id_;
}
const std::string& RosRobotLocalizationListener::getWorldFrameId() const
{
return world_frame_id_;
}
} // namespace RobotLocalization

458
Localizations/Packages/robot_localization/src/ukf.cpp Normal file
View File

@@ -0,0 +1,458 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ukf.h"
#include "robot_localization/filter_common.h"
#include <angles/angles.h>
#include <assert.h>
#include <Eigen/Cholesky>
#include <vector>
namespace RobotLocalization
{
Ukf::Ukf(std::vector<double> args) :
FilterBase(), // Must initialize filter base!
uncorrected_(true)
{
assert(args.size() == 3);
double alpha = args[0];
double kappa = args[1];
double beta = args[2];
size_t sigmaCount = (STATE_SIZE << 1) + 1;
sigmaPoints_.resize(sigmaCount, Eigen::VectorXd(STATE_SIZE));
// Prepare constants
lambda_ = alpha * alpha * (STATE_SIZE + kappa) - STATE_SIZE;
stateWeights_.resize(sigmaCount);
covarWeights_.resize(sigmaCount);
stateWeights_[0] = lambda_ / (STATE_SIZE + lambda_);
covarWeights_[0] = stateWeights_[0] + (1 - (alpha * alpha) + beta);
sigmaPoints_[0].setZero();
for (size_t i = 1; i < sigmaCount; ++i)
{
sigmaPoints_[i].setZero();
stateWeights_[i] = 1 / (2 * (STATE_SIZE + lambda_));
covarWeights_[i] = stateWeights_[i];
}
}
Ukf::~Ukf()
{
}
void Ukf::correct(const Measurement &measurement)
{
FB_DEBUG("---------------------- Ukf::correct ----------------------\n" <<
"State is:\n" << state_ <<
"\nMeasurement is:\n" << measurement.measurement_ <<
"\nMeasurement covariance is:\n" << measurement.covariance_ << "\n");
// In our implementation, it may be that after we call predict once, we call correct
// several times in succession (multiple measurements with different time stamps). In
// that event, the sigma points need to be updated to reflect the current state.
// Throughout prediction and correction, we attempt to maximize efficiency in Eigen.
if (!uncorrected_)
{
generateSigmaPoints();
}
// We don't want to update everything, so we need to build matrices that only update
// the measured parts of our state vector
// First, determine how many state vector values we're updating
std::vector<size_t> updateIndices;
for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
{
if (measurement.updateVector_[i])
{
// Handle nan and inf values in measurements
if (std::isnan(measurement.measurement_(i)))
{
FB_DEBUG("Value at index " << i << " was nan. Excluding from update.\n");
}
else if (std::isinf(measurement.measurement_(i)))
{
FB_DEBUG("Value at index " << i << " was inf. Excluding from update.\n");
}
else
{
updateIndices.push_back(i);
}
}
}
FB_DEBUG("Update indices are:\n" << updateIndices << "\n");
size_t updateSize = updateIndices.size();
// Now set up the relevant matrices
Eigen::VectorXd stateSubset(updateSize); // x (in most literature)
Eigen::VectorXd measurementSubset(updateSize); // z
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // R
Eigen::MatrixXd stateToMeasurementSubset(updateSize, STATE_SIZE); // H
Eigen::MatrixXd kalmanGainSubset(STATE_SIZE, updateSize); // K
Eigen::VectorXd innovationSubset(updateSize); // z - Hx
Eigen::VectorXd predictedMeasurement(updateSize);
Eigen::VectorXd sigmaDiff(updateSize);
Eigen::MatrixXd predictedMeasCovar(updateSize, updateSize);
Eigen::MatrixXd crossCovar(STATE_SIZE, updateSize);
std::vector<Eigen::VectorXd> sigmaPointMeasurements(sigmaPoints_.size(), Eigen::VectorXd(updateSize));
stateSubset.setZero();
measurementSubset.setZero();
measurementCovarianceSubset.setZero();
stateToMeasurementSubset.setZero();
kalmanGainSubset.setZero();
innovationSubset.setZero();
predictedMeasurement.setZero();
predictedMeasCovar.setZero();
crossCovar.setZero();
// Now build the sub-matrices from the full-sized matrices
for (size_t i = 0; i < updateSize; ++i)
{
measurementSubset(i) = measurement.measurement_(updateIndices[i]);
stateSubset(i) = state_(updateIndices[i]);
for (size_t j = 0; j < updateSize; ++j)
{
measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]);
}
// Handle negative (read: bad) covariances in the measurement. Rather
// than exclude the measurement or make up a covariance, just take
// the absolute value.
if (measurementCovarianceSubset(i, i) < 0)
{
FB_DEBUG("WARNING: Negative covariance for index " << i <<
" of measurement (value is" << measurementCovarianceSubset(i, i) <<
"). Using absolute value...\n");
measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
}
// If the measurement variance for a given variable is very
// near 0 (as in e-50 or so) and the variance for that
// variable in the covariance matrix is also near zero, then
// the Kalman gain computation will blow up. Really, no
// measurement can be completely without error, so add a small
// amount in that case.
if (measurementCovarianceSubset(i, i) < 1e-9)
{
measurementCovarianceSubset(i, i) = 1e-9;
FB_DEBUG("WARNING: measurement had very small error covariance for index " <<
updateIndices[i] <<
". Adding some noise to maintain filter stability.\n");
}
}
// The state-to-measurement function, h, will now be a measurement_size x full_state_size
// matrix, with ones in the (i, i) locations of the values to be updated
for (size_t i = 0; i < updateSize; ++i)
{
stateToMeasurementSubset(i, updateIndices[i]) = 1;
}
FB_DEBUG("Current state subset is:\n" << stateSubset <<
"\nMeasurement subset is:\n" << measurementSubset <<
"\nMeasurement covariance subset is:\n" << measurementCovarianceSubset <<
"\nState-to-measurement subset is:\n" << stateToMeasurementSubset << "\n");
double roll_sum_x {};
double roll_sum_y {};
double pitch_sum_x {};
double pitch_sum_y {};
double yaw_sum_x {};
double yaw_sum_y {};
// (1) Generate sigma points, use them to generate a predicted measurement
for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
sigmaPointMeasurements[sigmaInd] = stateToMeasurementSubset * sigmaPoints_[sigmaInd];
predictedMeasurement.noalias() += stateWeights_[sigmaInd] * sigmaPointMeasurements[sigmaInd];
// Euler angle averaging requires special care
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll)
{
roll_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPointMeasurements[sigmaInd](i));
roll_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPointMeasurements[sigmaInd](i));
}
else if (updateIndices[i] == StateMemberPitch)
{
pitch_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPointMeasurements[sigmaInd](i));
pitch_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPointMeasurements[sigmaInd](i));
}
else if (updateIndices[i] == StateMemberYaw)
{
yaw_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPointMeasurements[sigmaInd](i));
yaw_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPointMeasurements[sigmaInd](i));
}
}
}
// Wrap angles in the innovation
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll)
{
predictedMeasurement(i) = ::atan2(roll_sum_y, roll_sum_x);
}
else if (updateIndices[i] == StateMemberPitch)
{
predictedMeasurement(i) = ::atan2(pitch_sum_y, pitch_sum_x);
}
else if (updateIndices[i] == StateMemberYaw)
{
predictedMeasurement(i) = ::atan2(yaw_sum_y, yaw_sum_x);
}
}
// (2) Use the sigma point measurements and predicted measurement to compute a predicted
// measurement covariance matrix P_zz and a state/measurement cross-covariance matrix P_xz.
for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
sigmaDiff = sigmaPointMeasurements[sigmaInd] - predictedMeasurement;
Eigen::VectorXd sigmaStateDiff = sigmaPoints_[sigmaInd] - state_;
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll ||
updateIndices[i] == StateMemberPitch ||
updateIndices[i] == StateMemberYaw)
{
sigmaDiff(i) = angles::normalize_angle(sigmaDiff(i));
sigmaStateDiff(i) = angles::normalize_angle(sigmaStateDiff(i));
}
}
predictedMeasCovar.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
crossCovar.noalias() += covarWeights_[sigmaInd] * (sigmaStateDiff * sigmaDiff.transpose());
}
// (3) Compute the Kalman gain, making sure to use the actual measurement covariance: K = P_xz * (P_zz + R)^-1
Eigen::MatrixXd invInnovCov = (predictedMeasCovar + measurementCovarianceSubset).inverse();
kalmanGainSubset = crossCovar * invInnovCov;
// (4) Apply the gain to the difference between the actual and predicted measurements: x = x + K(z - z_hat)
innovationSubset = (measurementSubset - predictedMeasurement);
// Wrap angles in the innovation
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll ||
updateIndices[i] == StateMemberPitch ||
updateIndices[i] == StateMemberYaw)
{
innovationSubset(i) = angles::normalize_angle(innovationSubset(i));
}
}
// (5) Check Mahalanobis distance of innovation
if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisThresh_))
{
state_.noalias() += kalmanGainSubset * innovationSubset;
// (6) Compute the new estimate error covariance P = P - (K * P_zz * K')
estimateErrorCovariance_.noalias() -= (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose());
wrapStateAngles();
// Mark that we need to re-compute sigma points for successive corrections
uncorrected_ = false;
FB_DEBUG("Predicated measurement covariance is:\n" << predictedMeasCovar <<
"\nCross covariance is:\n" << crossCovar <<
"\nKalman gain subset is:\n" << kalmanGainSubset <<
"\nInnovation:\n" << innovationSubset <<
"\nCorrected full state is:\n" << state_ <<
"\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n---------------------- /Ukf::correct ----------------------\n");
}
}
void Ukf::predict(const double referenceTime, const double delta)
{
FB_DEBUG("---------------------- Ukf::predict ----------------------\n" <<
"delta is " << delta <<
"\nstate is " << state_ << "\n");
prepareControl(referenceTime, delta);
generateSigmaPoints();
double roll_sum_x {};
double roll_sum_y {};
double pitch_sum_x {};
double pitch_sum_y {};
double yaw_sum_x {};
double yaw_sum_y {};
// Sum the weighted sigma points to generate a new state prediction
state_.setZero();
for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
// Apply the state transition function to this sigma point
projectSigmaPoint(sigmaPoints_[sigmaInd], delta);
state_.noalias() += stateWeights_[sigmaInd] * sigmaPoints_[sigmaInd];
// Euler angle averaging requires special care
roll_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPoints_[sigmaInd](StateMemberRoll));
roll_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPoints_[sigmaInd](StateMemberRoll));
pitch_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPoints_[sigmaInd](StateMemberPitch));
pitch_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPoints_[sigmaInd](StateMemberPitch));
yaw_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPoints_[sigmaInd](StateMemberYaw));
yaw_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPoints_[sigmaInd](StateMemberYaw));
}
// Recover average Euler angles
state_(StateMemberRoll) = ::atan2(roll_sum_y, roll_sum_x);
state_(StateMemberPitch) = ::atan2(pitch_sum_y, pitch_sum_x);
state_(StateMemberYaw) = ::atan2(yaw_sum_y, yaw_sum_x);
// Now use the sigma points and the predicted state to compute a predicted covariance
estimateErrorCovariance_.setZero();
Eigen::VectorXd sigmaDiff(STATE_SIZE);
for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
sigmaDiff = (sigmaPoints_[sigmaInd] - state_);
sigmaDiff(StateMemberRoll) = angles::normalize_angle(sigmaDiff(StateMemberRoll));
sigmaDiff(StateMemberPitch) = angles::normalize_angle(sigmaDiff(StateMemberPitch));
sigmaDiff(StateMemberYaw) = angles::normalize_angle(sigmaDiff(StateMemberYaw));
estimateErrorCovariance_.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
}
// Not strictly in the theoretical UKF formulation, but necessary here
// to ensure that we actually incorporate the processNoiseCovariance_
Eigen::MatrixXd *processNoiseCovariance = &processNoiseCovariance_;
if (useDynamicProcessNoiseCovariance_)
{
computeDynamicProcessNoiseCovariance(state_, delta);
processNoiseCovariance = &dynamicProcessNoiseCovariance_;
}
estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);
// Keep the angles bounded
wrapStateAngles();
// Mark that we can keep these sigma points
uncorrected_ = true;
FB_DEBUG("Predicted state is:\n" << state_ <<
"\nPredicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n--------------------- /Ukf::predict ----------------------\n");
}
void Ukf::generateSigmaPoints()
{
// Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition
weightedCovarSqrt_ = ((static_cast<double>(STATE_SIZE) + lambda_) * estimateErrorCovariance_).llt().matrixL();
// Compute sigma points
// First sigma point is the current state
sigmaPoints_[0] = state_;
// Next STATE_SIZE sigma points are state + weightedCovarSqrt_[ith column]
// STATE_SIZE sigma points after that are state - weightedCovarSqrt_[ith column]
for (size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
{
sigmaPoints_[sigmaInd + 1] = state_ + weightedCovarSqrt_.col(sigmaInd);
sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = state_ - weightedCovarSqrt_.col(sigmaInd);
}
}
void Ukf::projectSigmaPoint(Eigen::VectorXd& sigmaPoint, double delta)
{
double roll = sigmaPoint(StateMemberRoll);
double pitch = sigmaPoint(StateMemberPitch);
double yaw = sigmaPoint(StateMemberYaw);
// We'll need these trig calculations a lot.
double sp = ::sin(pitch);
double cp = ::cos(pitch);
double cpi = 1.0 / cp;
double tp = sp * cpi;
double sr = ::sin(roll);
double cr = ::cos(roll);
double sy = ::sin(yaw);
double cy = ::cos(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
sigmaPoint.applyOnTheLeft(transferFunction_);
}
} // namespace RobotLocalization

57
Localizations/Packages/robot_localization/src/ukf_localization_node.cpp Normal file
View File

@@ -0,0 +1,57 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <ros/ros.h>
#include <cstdlib>
#include <vector>
int main(int argc, char **argv)
{
ros::init(argc, argv, "ukf_navigation_node");
ros::NodeHandle nh;
ros::NodeHandle nhLocal("~");
std::vector<double> args(3, 0);
nhLocal.param("alpha", args[0], 0.001);
nhLocal.param("kappa", args[1], 0.0);
nhLocal.param("beta", args[2], 2.0);
RobotLocalization::RosUkf ukf(nh, nhLocal, args);
ukf.initialize();
ros::spin();
return EXIT_SUCCESS;
}

71
Localizations/Packages/robot_localization/src/ukf_localization_nodelet.cpp Normal file
View File

@@ -0,0 +1,71 @@
/*
* Copyright (c) 2017 Simon Gene Gottlieb
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <memory>
#include <vector>
namespace RobotLocalization
{
class UkfNodelet : public nodelet::Nodelet
{
private:
std::unique_ptr<RosUkf> ukf;
public:
virtual void onInit()
{
NODELET_DEBUG("Initializing nodelet...");
ros::NodeHandle nh = getNodeHandle();
ros::NodeHandle nh_priv = getPrivateNodeHandle();
std::vector<double> args(3, 0);
nh_priv.param("alpha", args[0], 0.001);
nh_priv.param("kappa", args[1], 0.0);
nh_priv.param("beta", args[2], 2.0);
ukf = std::make_unique<RosUkf>(nh, nh_priv, getName(), args);
ukf->initialize();
}
};
} // namespace RobotLocalization
PLUGINLIB_EXPORT_CLASS(RobotLocalization::UkfNodelet, nodelet::Nodelet);

3
Localizations/Packages/robot_localization/srv/FromLL.srv Normal file
View File

@@ -0,0 +1,3 @@
geographic_msgs/GeoPoint ll_point
---
geometry_msgs/Point map_point

8
Localizations/Packages/robot_localization/srv/GetState.srv Normal file
View File

@@ -0,0 +1,8 @@
time time_stamp
string frame_id
---
# State vector: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
float64[15] state
# Covariance matrix in row-major order
float64[225] covariance

2
Localizations/Packages/robot_localization/srv/SetDatum.srv Normal file
View File

@@ -0,0 +1,2 @@
geographic_msgs/GeoPose geo_pose
---

2
Localizations/Packages/robot_localization/srv/SetPose.srv Normal file
View File

@@ -0,0 +1,2 @@
geometry_msgs/PoseWithCovarianceStamped pose
---

4
Localizations/Packages/robot_localization/srv/SetUTMZone.srv Normal file
View File

@@ -0,0 +1,4 @@
# Set an UTM zone navsat_transform should stick to.
# Example: 31U
string utm_zone
---

3
Localizations/Packages/robot_localization/srv/ToLL.srv Normal file
View File

@@ -0,0 +1,3 @@
geometry_msgs/Point map_point
---
geographic_msgs/GeoPoint ll_point

3
Localizations/Packages/robot_localization/srv/ToggleFilterProcessing.srv Normal file
View File

@@ -0,0 +1,3 @@
bool on
---
bool status

9
Localizations/Packages/robot_localization/test/record_all_stats.sh Normal file
View File

@@ -0,0 +1,9 @@
#!/bin/bash
./stat_recorder.sh test_ekf_localization_node_bag1.test $1/ekf1.txt
./stat_recorder.sh test_ekf_localization_node_bag2.test $1/ekf2.txt
./stat_recorder.sh test_ekf_localization_node_bag3.test $1/ekf3.txt
./stat_recorder.sh test_ukf_localization_node_bag1.test $1/ukf1.txt
./stat_recorder.sh test_ukf_localization_node_bag2.test $1/ukf2.txt
./stat_recorder.sh test_ukf_localization_node_bag3.test $1/ukf3.txt

18
Localizations/Packages/robot_localization/test/stat_recorder.sh Normal file
View File

@@ -0,0 +1,18 @@
#!/bin/bash
rm $2
echo "x = [" > $2
i="0"
while [ $i -lt 30 ]
do
rostest robot_localization $1 output_final_position:=true output_location:=$2
i=$[$i+1]
sleep 3
done
echo "]" >> $2
echo "mean(x)" >> $2
echo "sqrt(sum((4 * std(x)).^2))" >> $2

BIN
Localizations/Packages/robot_localization/test/test1.bag Normal file
View File

Binary file not shown.

BIN
Localizations/Packages/robot_localization/test/test2.bag Normal file
View File

Binary file not shown.

BIN
Localizations/Packages/robot_localization/test/test3.bag Normal file
View File

Binary file not shown.

135
Localizations/Packages/robot_localization/test/test_ekf.cpp Normal file
View File

@@ -0,0 +1,135 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <gtest/gtest.h>
#include <limits>
#include <vector>
using RobotLocalization::Ekf;
using RobotLocalization::RosEkf;
using RobotLocalization::STATE_SIZE;
class RosEkfPassThrough : public RosEkf
{
public:
RosEkfPassThrough() : RosEkf(ros::NodeHandle(), ros::NodeHandle("~"))
{
}
Ekf &getFilter()
{
return filter_;
}
};
TEST(EkfTest, Measurements)
{
RosEkfPassThrough ekf;
Eigen::MatrixXd initialCovar(15, 15);
initialCovar.setIdentity();
initialCovar *= 0.5;
ekf.getFilter().setEstimateErrorCovariance(initialCovar);
Eigen::VectorXd measurement(STATE_SIZE);
measurement.setIdentity();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurement[i] = i * 0.01 * STATE_SIZE;
}
Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
measurementCovariance.setIdentity();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
std::vector<int> updateVector(STATE_SIZE, true);
// Ensure that measurements are being placed in the queue correctly
ros::Time time;
time.fromSec(1000);
ekf.enqueueMeasurement("odom0",
measurement,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ekf.integrateMeasurements(ros::Time(1001));
EXPECT_EQ(ekf.getFilter().getState(), measurement);
EXPECT_EQ(ekf.getFilter().getEstimateErrorCovariance(), measurementCovariance);
ekf.getFilter().setEstimateErrorCovariance(initialCovar);
// Now fuse another measurement and check the output.
// We know what the filter's state should be when
// this is complete, so we'll check the difference and
// make sure it's suitably small.
Eigen::VectorXd measurement2 = measurement;
measurement2 *= 2.0;
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
time.fromSec(1002);
ekf.enqueueMeasurement("odom0",
measurement2,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ekf.integrateMeasurements(ros::Time(1003));
measurement = measurement2.eval() - ekf.getFilter().getState();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
EXPECT_LT(::fabs(measurement[i]), 0.001);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ekf");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

7
Localizations/Packages/robot_localization/test/test_ekf.test Normal file
View File

@@ -0,0 +1,7 @@
<!-- Launch file for ekf_test -->
<launch>
<test test-name="test_ekf" pkg="robot_localization" type="test_ekf" clear_params="true" time-limit="100.0" />
</launch>

105
Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag1.test Normal file
View File

@@ -0,0 +1,105 @@
<!-- Launch file for test_ekf_localization_node_bag1 -->
<!-- In this run, we ran a Clearpath Husky with a slightly broken Microstrain 3DM-GX2 IMU
around a parking lot. The IMU was intentionally rotated +90 degrees about the X axis,
then rotated (extrinsically) -90 degrees about the Z axis. -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test1.bag --clock -d 5" required="true"/>
<node name="test_ekf_localization_node_bag1_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag1_pose" pkg="robot_localization" type="test_ekf_localization_node_bag1" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-40.0454"/>
<param name="final_y" value="-76.9988"/>
<param name="final_z" value="-2.6974"/>
<param name="tolerance" value="1.0452"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

85
Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag2.test Normal file
View File

@@ -0,0 +1,85 @@
<!-- Launch file for test_ekf_localization_node_bag2 -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test2.bag --clock -d 5" required="true"/>
<node name="test_ekf_localization_node_bag2_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true" >
<param name="frequency" value="50"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/jackal_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="process_noise_covariance">[0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag2_pose" pkg="robot_localization" type="test_ekf_localization_node_bag2" clear_params="true" time-limit="1000.0">
<param name="final_x" value="1.0206"/>
<param name="final_y" value="3.4292"/>
<param name="final_z" value="0.7419"/>
<param name="tolerance" value="0.1383"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

69
Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag3.test Normal file
View File

@@ -0,0 +1,69 @@
<!-- Launch file for test_ekf_localization_node_bag3_ekf" -->
<launch>
<arg name="output_final_position" default="false"/>
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test3.bag --clock -d 5" required="true"/>
<node name="test_ekf_localization_node_bag3_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/ardrone/odometry/raw"/>
<param name="imu0" value="/ardrone/imu"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="odom0_config">[false, false, true,
false, false, false,
true, true, false,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]</rosparam>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag3_pose" pkg="robot_localization" type="test_ekf_localization_node_bag3" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-0.4859"/>
<param name="final_y" value="-0.2412"/>
<param name="final_z" value="2.9883"/>
<param name="tolerance" value="0.1290"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

1075
Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

68
Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.test Normal file
View File

@@ -0,0 +1,68 @@
<!-- Launch file for test_ekf_localization_node_interfaces-->
<launch>
<node name="test_ekf_localization_node_interfaces_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/odom_input0"/>
<param name="odom1" value="/odom_input1"/>
<param name="odom2" value="/odom_input2"/>
<param name="pose0" value="/pose_input0"/>
<param name="pose1" value="/pose_input1"/>
<param name="twist0" value="/twist_input0"/>
<param name="imu0" value="/imu_input0"/>
<param name="imu1" value="/imu_input1"/>
<param name="imu2" value="/imu_input2"/>
<param name="imu3" value="/imu_input3"/>
<rosparam param="odom0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom2_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="pose0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="pose1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="imu1_config">[false, false, false, false, false, false, false, false, false, true, true, true, false, false, false]</rosparam>
<rosparam param="imu2_config">[false, false, false, false, false, false, false, false, false, false, false, false, true, true, true]</rosparam>
<rosparam param="imu3_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<param name="odom1_differential" value="true"/>
<param name="pose1_differential" value="true"/>
<param name="imu3_differential" value="true"/>
<param name="print_diagnostics" value="false"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
<test test-name="test_ekf_localization_node_interfaces_int" pkg="robot_localization" type="test_ekf_localization_node_interfaces" clear_params="true" time-limit="1000.0" />
</launch>

104
Localizations/Packages/robot_localization/test/test_ekf_localization_nodelet_bag1.test Normal file
View File

@@ -0,0 +1,104 @@
<!-- Launch file for test_ekf_localization_node_bag1 -->
<!-- In this run, we ran a Clearpath Husky with a slightly broken Microstrain 3DM-GX2 IMU
around a parking lot. The IMU was intentionally rotated +90 degrees about the X axis,
then rotated (extrinsically) -90 degrees about the Z axis. -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test1.bag --clock -d 25" required="true"/>
<node pkg="nodelet" type="nodelet" name="EKF_NODELET_MANAGER" args="manager"/>
<node pkg="nodelet" type="nodelet" name="test_ekf_localization_node_bag1_ekf" output="screen" args="load RobotLocalization/EkfNodelet EKF_NODELET_MANAGER" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag1_pose" pkg="robot_localization" type="test_ekf_localization_node_bag1" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-40.0454"/>
<param name="final_y" value="-76.9988"/>
<param name="final_z" value="-2.6974"/>
<param name="tolerance" value="1.0452"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

216
Localizations/Packages/robot_localization/test/test_filter_base.cpp Normal file
View File

@@ -0,0 +1,216 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/filter_base.h"
#include "robot_localization/filter_common.h"
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <iostream>
#include <queue>
#include <string>
using RobotLocalization::STATE_SIZE;
using RobotLocalization::Measurement;
namespace RobotLocalization
{
class FilterDerived : public FilterBase
{
public:
double val;
FilterDerived() : val(0) { }
void correct(const Measurement &measurement)
{
EXPECT_EQ(val, measurement.time_);
EXPECT_EQ(measurement.topicName_, "topic");
EXPECT_EQ(measurement.updateVector_.size(), 10u);
for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
{
EXPECT_EQ(measurement.updateVector_[i], true);
}
}
void predict(const double refTime, const double delta)
{
val = delta;
}
};
class FilterDerived2 : public FilterBase
{
public:
FilterDerived2() { }
void correct(const Measurement &measurement)
{
}
void predict(const double refTime, const double delta)
{
}
void processMeasurement(const Measurement &measurement)
{
FilterBase::processMeasurement(measurement);
}
};
} // namespace RobotLocalization
TEST(FilterBaseTest, MeasurementStruct)
{
RobotLocalization::Measurement meas1;
RobotLocalization::Measurement meas2;
EXPECT_EQ(meas1.topicName_, std::string(""));
EXPECT_EQ(meas1.time_, 0);
EXPECT_EQ(meas2.time_, 0);
// Comparison test is true if the first
// argument is > the second, so should
// be false if they're equal.
EXPECT_EQ(meas1(meas1, meas2), false);
EXPECT_EQ(meas2(meas2, meas1), false);
meas1.time_ = 100;
meas2.time_ = 200;
EXPECT_EQ(meas1(meas1, meas2), false);
EXPECT_EQ(meas1(meas2, meas1), true);
EXPECT_EQ(meas2(meas1, meas2), false);
EXPECT_EQ(meas2(meas2, meas1), true);
}
TEST(FilterBaseTest, DerivedFilterGetSet)
{
using RobotLocalization::FilterDerived;
FilterDerived derived;
// With the ostream argument as NULL,
// the debug flag will remain false.
derived.setDebug(true);
EXPECT_FALSE(derived.getDebug());
// Now set the stream and do it again
std::stringstream os;
derived.setDebug(true, &os);
EXPECT_TRUE(derived.getDebug());
// Simple get/set checks
double timeout = 7.4;
derived.setSensorTimeout(timeout);
EXPECT_EQ(derived.getSensorTimeout(), timeout);
double lastMeasTime = 3.83;
derived.setLastMeasurementTime(lastMeasTime);
EXPECT_EQ(derived.getLastMeasurementTime(), lastMeasTime);
Eigen::MatrixXd pnCovar(STATE_SIZE, STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
for (size_t j = 0; j < STATE_SIZE; ++j)
{
pnCovar(i, j) = static_cast<double>(i * j);
}
}
derived.setProcessNoiseCovariance(pnCovar);
EXPECT_EQ(derived.getProcessNoiseCovariance(), pnCovar);
Eigen::VectorXd state(STATE_SIZE);
state.setZero();
derived.setState(state);
EXPECT_EQ(derived.getState(), state);
EXPECT_EQ(derived.getInitializedStatus(), false);
}
TEST(FilterBaseTest, MeasurementProcessing)
{
using RobotLocalization::FilterDerived2;
FilterDerived2 derived;
Measurement meas;
Eigen::VectorXd measurement(STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurement[i] = 0.1 * static_cast<double>(i);
}
Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
for (size_t j = 0; j < STATE_SIZE; ++j)
{
measurementCovariance(i, j) = 0.1 * static_cast<double>(i * j);
}
}
meas.topicName_ = "odomTest";
meas.measurement_ = measurement;
meas.covariance_ = measurementCovariance;
meas.updateVector_.resize(STATE_SIZE, true);
meas.time_ = 1000;
// The filter shouldn't be initializedyet
EXPECT_FALSE(derived.getInitializedStatus());
derived.processMeasurement(meas);
// Now it's initialized, and the entire filter state
// should be equal to the first state
EXPECT_TRUE(derived.getInitializedStatus());
EXPECT_EQ(derived.getState(), measurement);
// Process a measurement and make sure it updates the
// lastMeasurementTime variable
meas.time_ = 1002;
derived.processMeasurement(meas);
EXPECT_EQ(derived.getLastMeasurementTime(), meas.time_);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

434
Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.cpp Normal file
View File

@@ -0,0 +1,434 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <sensor_msgs/Imu.h>
#include "robot_localization/filter_base.h"
#include "robot_localization/filter_common.h"
#include "robot_localization/SetPose.h"
#include <diagnostic_msgs/DiagnosticArray.h>
#include <gtest/gtest.h>
#include <vector>
namespace RobotLocalization
{
/*
Convenience functions to get valid messages.
*/
geometry_msgs::PoseWithCovarianceStamped getValidPose()
{
geometry_msgs::PoseWithCovarianceStamped pose_msg;
pose_msg.header.frame_id = "base_link";
pose_msg.pose.pose.position.x = 1;
pose_msg.pose.pose.orientation.w = 1;
for (size_t i = 0; i < 6 ; i++)
{
pose_msg.pose.covariance[i*6 + i] = 1;
}
return pose_msg;
}
geometry_msgs::TwistWithCovarianceStamped getValidTwist()
{
geometry_msgs::TwistWithCovarianceStamped twist_msg;
twist_msg.header.frame_id = "base_link";
for (size_t i = 0; i < 6 ; i++)
{
twist_msg.twist.covariance[i*6 + i] = 1;
}
return twist_msg;
}
sensor_msgs::Imu getValidImu()
{
sensor_msgs::Imu imu_msg;
imu_msg.header.frame_id = "base_link";
imu_msg.orientation.w = 1;
for (size_t i = 0; i < 3 ; i++)
{
imu_msg.orientation_covariance[i * 3 + i] = 1;
imu_msg.angular_velocity_covariance[i * 3 + i] = 1;
imu_msg.linear_acceleration_covariance[i * 3 + i] = 1;
}
return imu_msg;
}
nav_msgs::Odometry getValidOdometry()
{
nav_msgs::Odometry odom_msg;
odom_msg.header.frame_id = "odom";
odom_msg.child_frame_id = "base_link";
odom_msg.pose = getValidPose().pose;
odom_msg.twist = getValidTwist().twist;
return odom_msg;
}
/*
Helper class to handle the setup and message publishing for the testcases.
It provides convenience to send valid messages with a specified timestamp.
All diagnostic messages are stored into the public diagnostics attribute.
*/
class DiagnosticsHelper
{
private:
ros::Publisher odom_pub_;
ros::Publisher pose_pub_;
ros::Publisher twist_pub_;
ros::Publisher imu_pub_;
geometry_msgs::PoseWithCovarianceStamped pose_msg_;
geometry_msgs::TwistWithCovarianceStamped twist_msg_;
nav_msgs::Odometry odom_msg_;
sensor_msgs::Imu imu_msg_;
ros::Subscriber diagnostic_sub_;
ros::ServiceClient set_pose_;
public:
std::vector< diagnostic_msgs::DiagnosticArray > diagnostics;
DiagnosticsHelper()
{
ros::NodeHandle nh;
ros::NodeHandle nhLocal("~");
// ready the valid messages.
pose_msg_ = getValidPose();
twist_msg_ = getValidTwist();
odom_msg_ = getValidOdometry();
imu_msg_ = getValidImu();
// subscribe to diagnostics and create publishers for the odometry messages.
odom_pub_ = nh.advertise<nav_msgs::Odometry>("example/odom", 10);
pose_pub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("example/pose", 10);
twist_pub_ = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("example/twist", 10);
imu_pub_ = nh.advertise<sensor_msgs::Imu>("example/imu/data", 10);
diagnostic_sub_ = nh.subscribe("/diagnostics", 10, &DiagnosticsHelper::diagnosticCallback, this);
set_pose_ = nh.serviceClient<robot_localization::SetPose>("/set_pose");
}
void diagnosticCallback(const diagnostic_msgs::DiagnosticArrayPtr &msg)
{
diagnostics.push_back(*msg);
}
void publishMessages(ros::Time t)
{
odom_msg_.header.stamp = t;
odom_msg_.header.seq++;
odom_pub_.publish(odom_msg_);
pose_msg_.header.stamp = t;
pose_msg_.header.seq++;
pose_pub_.publish(pose_msg_);
twist_msg_.header.stamp = t;
twist_msg_.header.seq++;
twist_pub_.publish(twist_msg_);
imu_msg_.header.stamp = t;
imu_msg_.header.seq++;
imu_pub_.publish(imu_msg_);
}
void setPose(ros::Time t)
{
robot_localization::SetPose pose_;
pose_.request.pose = getValidPose();
pose_.request.pose.header.stamp = t;
set_pose_.call(pose_);
}
};
} // namespace RobotLocalization
/*
First test; we run for a bit; then send messagse with an empty timestamp.
Then we check if the diagnostics showed a warning.
*/
TEST(FilterBaseDiagnosticsTest, EmptyTimestamps)
{
RobotLocalization::DiagnosticsHelper dh_;
// keep track of which diagnostic messages are detected.
bool received_warning_imu = false;
bool received_warning_odom = false;
bool received_warning_twist = false;
bool received_warning_pose = false;
// For about a second, send correct messages.
ros::Rate loopRate(10);
for (size_t i = 0; i < 10; ++i)
{
ros::spinOnce();
dh_.publishMessages(ros::Time::now());
loopRate.sleep();
}
ros::spinOnce();
// create an empty timestamp and send all messages with this empty timestamp.
ros::Time empty;
empty.fromSec(0);
dh_.publishMessages(empty);
ros::spinOnce();
// The filter runs and sends the diagnostics every second.
// Just run this for two seconds to ensure we get all the diagnostic message.
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
loopRate.sleep();
}
/*
Now the diagnostic messages have to be investigated to see whether they contain our warning.
*/
for (size_t i=0; i < dh_.diagnostics.size(); i++)
{
for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++)
{
for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++)
{
diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key];
// Now the keys can be checked to see whether we found our warning.
if (kv.key == "imu0_timestamp")
{
received_warning_imu = true;
}
if (kv.key == "odom0_timestamp")
{
received_warning_odom = true;
}
if (kv.key == "twist0_timestamp")
{
received_warning_twist = true;
}
if (kv.key == "pose0_timestamp")
{
received_warning_pose = true;
}
}
}
}
EXPECT_TRUE(received_warning_imu);
EXPECT_TRUE(received_warning_odom);
EXPECT_TRUE(received_warning_twist);
EXPECT_TRUE(received_warning_pose);
}
TEST(FilterBaseDiagnosticsTest, TimestampsBeforeSetPose)
{
RobotLocalization::DiagnosticsHelper dh_;
// keep track of which diagnostic messages are detected.
bool received_warning_imu = false;
bool received_warning_odom = false;
bool received_warning_twist = false;
bool received_warning_pose = false;
// For about a second, send correct messages.
ros::Rate loopRate(10);
for (size_t i = 0; i < 10; ++i)
{
ros::spinOnce();
dh_.publishMessages(ros::Time::now());
loopRate.sleep();
}
ros::spinOnce();
// Set the pose to the current timestamp.
dh_.setPose(ros::Time::now());
ros::spinOnce();
// send messages from one second before that pose reset.
dh_.publishMessages(ros::Time::now() - ros::Duration(1));
// The filter runs and sends the diagnostics every second.
// Just run this for two seconds to ensure we get all the diagnostic message.
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
loopRate.sleep();
}
/*
Now the diagnostic messages have to be investigated to see whether they contain our warning.
*/
for (size_t i=0; i < dh_.diagnostics.size(); i++)
{
for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++)
{
for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++)
{
diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key];
// Now the keys can be checked to see whether we found our warning.
if (kv.key == "imu0_timestamp")
{
received_warning_imu = true;
}
if (kv.key == "odom0_timestamp")
{
received_warning_odom = true;
}
if (kv.key == "twist0_timestamp")
{
received_warning_twist = true;
}
if (kv.key == "pose0_timestamp")
{
received_warning_pose = true;
}
}
}
}
EXPECT_TRUE(received_warning_imu);
EXPECT_TRUE(received_warning_odom);
EXPECT_TRUE(received_warning_twist);
EXPECT_TRUE(received_warning_pose);
}
TEST(FilterBaseDiagnosticsTest, TimestampsBeforePrevious)
{
RobotLocalization::DiagnosticsHelper dh_;
// keep track of which diagnostic messages are detected.
// we have more things to check here because the messages get split over
// various callbacks if they pass the check if they predate the set_pose time.
bool received_warning_imu_accel = false;
bool received_warning_imu_pose = false;
bool received_warning_imu_twist = false;
bool received_warning_odom_twist = false;
bool received_warning_twist = false;
bool received_warning_pose = false;
// For two seconds send correct messages.
ros::Rate loopRate(10);
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
dh_.publishMessages(ros::Time::now());
loopRate.sleep();
}
ros::spinOnce();
// Send message that is one second in the past.
dh_.publishMessages(ros::Time::now() - ros::Duration(1));
// The filter runs and sends the diagnostics every second.
// Just run this for two seconds to ensure we get all the diagnostic message.
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
loopRate.sleep();
}
/*
Now the diagnostic messages have to be investigated to see whether they contain our warning.
*/
for (size_t i=0; i < dh_.diagnostics.size(); i++)
{
for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++)
{
for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++)
{
diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key];
// Now the keys can be checked to see whether we found our warning.
if (kv.key == "imu0_acceleration_timestamp")
{
received_warning_imu_accel = true;
}
if (kv.key == "imu0_pose_timestamp")
{
received_warning_imu_pose = true;
}
if (kv.key == "imu0_twist_timestamp")
{
received_warning_imu_twist = true;
}
if (kv.key == "odom0_twist_timestamp")
{
received_warning_twist = true;
}
if (kv.key == "pose0_timestamp")
{
received_warning_pose = true;
}
if (kv.key == "twist0_timestamp")
{
received_warning_odom_twist = true;
}
}
}
}
EXPECT_TRUE(received_warning_imu_accel);
EXPECT_TRUE(received_warning_imu_pose);
EXPECT_TRUE(received_warning_imu_twist);
EXPECT_TRUE(received_warning_odom_twist);
EXPECT_TRUE(received_warning_pose);
EXPECT_TRUE(received_warning_twist);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "filter_base_diagnostics_timestamps-test-interfaces");
ros::Time::init();
// Give ekf_localization_node time to initialize
ros::Duration(2.0).sleep();
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

44
Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.test Normal file
View File

@@ -0,0 +1,44 @@
<!-- Launch file for test_filter_base_diagnostics -->
<launch>
<!--
Although we test the filter base we need a valid node running which sends the diagnostic messages.
-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<param name="odom0" value="example/odom"/>
<param name="pose0" value="example/pose"/>
<param name="twist0" value="example/twist"/>
<param name="imu0" value="example/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="pose0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="twist0_config">[false, false, false,
false, false, false,
true, true, true,
true, true, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="print_diagnostics" value="true"/>
</node>
<test test-name="test_filter_base_diagnostics" pkg="robot_localization" type="test_filter_base_diagnostics_timestamps" clear_params="true" time-limit="100.0" />
</launch>

111
Localizations/Packages/robot_localization/test/test_localization_node_bag_pose_tester.cpp Normal file
View File

@@ -0,0 +1,111 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <gtest/gtest.h>
#include <fstream>
#include <iostream>
#include <string>
nav_msgs::Odometry filtered_;
void filterCallback(const nav_msgs::OdometryConstPtr &msg)
{
filtered_ = *msg;
}
TEST(BagTest, PoseCheck)
{
ros::NodeHandle nh;
ros::NodeHandle nhLocal("~");
double finalX = 0;
double finalY = 0;
double finalZ = 0;
double tolerance = 0;
bool outputFinalPosition = false;
std::string finalPositionFile;
nhLocal.getParam("final_x", finalX);
nhLocal.getParam("final_y", finalY);
nhLocal.getParam("final_z", finalZ);
nhLocal.getParam("tolerance", tolerance);
nhLocal.param("output_final_position", outputFinalPosition, false);
nhLocal.param("output_location", finalPositionFile, std::string("test.txt"));
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
while (ros::ok())
{
ros::spinOnce();
ros::Duration(0.0333333).sleep();
}
if (outputFinalPosition)
{
try
{
std::ofstream posOut;
posOut.open(finalPositionFile.c_str(), std::ofstream::app);
posOut << filtered_.pose.pose.position.x << " " << filtered_.pose.pose.position.y << " " <<
filtered_.pose.pose.position.z << std::endl;
posOut.close();
}
catch(...)
{
ROS_ERROR_STREAM("Unable to open output file.\n");
}
}
double xDiff = filtered_.pose.pose.position.x - finalX;
double yDiff = filtered_.pose.pose.position.y - finalY;
double zDiff = filtered_.pose.pose.position.z - finalZ;
EXPECT_LT(::sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff), tolerance);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "localization_node-bag-pose-tester");
ros::Time::init();
// Give ekf_localization_node time to initialize
ros::Duration(2.0).sleep();
return RUN_ALL_TESTS();
}

69
Localizations/Packages/robot_localization/test/test_navsat_conversions.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*
* Copyright (c) 2021, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/navsat_conversions.h"
#include <gtest/gtest.h>
#include <string>
void NavsatConversionsTest(const double lat, const double lon,
const double UTMNorthing, const double UTMEasting,
const std::string UTMZone, const double gamma)
{
double UTMNorthing_new;
double UTMEasting_new;
std::string UTMZone_new;
double gamma_new;
RobotLocalization::NavsatConversions::LLtoUTM(lat, lon, UTMNorthing_new, UTMEasting_new, UTMZone_new, gamma_new);
EXPECT_NEAR(UTMNorthing, UTMNorthing_new, 1e-2);
EXPECT_NEAR(UTMEasting, UTMEasting_new, 1e-2);
EXPECT_EQ(UTMZone, UTMZone_new);
EXPECT_NEAR(gamma, gamma_new, 1e-2);
double lat_new;
double lon_new;
RobotLocalization::NavsatConversions::UTMtoLL(UTMNorthing, UTMEasting, UTMZone, lat_new, lon_new);
EXPECT_NEAR(lat_new, lat, 1e-5);
EXPECT_NEAR(lon_new, lon, 1e-5);
}
TEST(NavsatConversionsTest, UtmTest)
{
NavsatConversionsTest(51.423964, 5.494271, 5699924.709, 673409.989, "31U", 1.950);
NavsatConversionsTest(-43.530955, 172.636645, 5178919.718, 632246.802, "59G", -1.127);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

91
Localizations/Packages/robot_localization/test/test_navsat_transform.cpp Normal file
View File

@@ -0,0 +1,91 @@
/*
* Copyright (c) 2021, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/navsat_transform.h"
#include <robot_localization/SetDatum.h>
#include <robot_localization/ToLL.h>
#include <robot_localization/FromLL.h>
#include <gtest/gtest.h>
#include <string>
TEST(NavSatTransformUTMJumpTest, UtmTest)
{
ros::NodeHandle nh;
ros::ServiceClient set_datum_client = nh.serviceClient<robot_localization::SetDatum>("/datum");
ros::ServiceClient from_ll_client = nh.serviceClient<robot_localization::FromLL>("/fromLL");
EXPECT_TRUE(set_datum_client.waitForExistence(ros::Duration(5)));
// Initialise the navsat_transform node to a UTM zone
robot_localization::SetDatum set_datum_srv;
set_datum_srv.request.geo_pose.position.latitude = 1;
set_datum_srv.request.geo_pose.position.longitude = 4;
set_datum_srv.request.geo_pose.orientation.w = 1;
EXPECT_TRUE(set_datum_client.call(set_datum_srv));
// Let the node figure out its transforms
ros::Duration(0.2).sleep();
// Request the GPS point of said point:
robot_localization::FromLL from_ll_srv;
from_ll_srv.request.ll_point.latitude = 10;
from_ll_srv.request.ll_point.longitude = 4.5;
EXPECT_TRUE(from_ll_client.call(from_ll_srv));
auto initial_response = from_ll_srv.response;
// Request GPS point also in that zone
from_ll_srv.request.ll_point.longitude = 5.5;
EXPECT_TRUE(from_ll_client.call(from_ll_srv));
auto same_zone_response = from_ll_srv.response;
// 1° of longitude is about 111 kilometers in length
EXPECT_NEAR(initial_response.map_point.x, same_zone_response.map_point.x, 120e3);
EXPECT_NEAR(initial_response.map_point.y, same_zone_response.map_point.y, 120e3);
// Request GPS point from neighboring zone (zone crossing is at 6 degrees)
from_ll_srv.request.ll_point.longitude = 6.5;
from_ll_client.call(from_ll_srv);
auto neighbour_zone_response = from_ll_srv.response;
EXPECT_NEAR(initial_response.map_point.x, neighbour_zone_response.map_point.x, 2*120e3);
EXPECT_NEAR(initial_response.map_point.y, neighbour_zone_response.map_point.y, 2*120e3);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_navsat_transform");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

9
Localizations/Packages/robot_localization/test/test_navsat_transform.test Normal file
View File

@@ -0,0 +1,9 @@
<!-- Launch file for navsat_transform test -->
<launch>
<node name="navsat_transform_node" pkg="robot_localization" type="navsat_transform_node" output="screen" />
<test test-name="test_navsat_transform" pkg="robot_localization" type="test_navsat_transform" />
</launch>

169
Localizations/Packages/robot_localization/test/test_robot_localization_estimator.cpp Normal file
View File

@@ -0,0 +1,169 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/robot_localization_estimator.h"
#include <vector>
#include <ros/ros.h>
#include <gtest/gtest.h>
TEST(RLETest, StateBuffer)
{
// Generate a few estimator states
std::vector<RobotLocalization::EstimatorState> states;
for ( int i = 0; i < 10; i++ )
{
/*
* t = i s;
* x = i m;
* vx = 1.0 m/s;
*/
RobotLocalization::EstimatorState state;
state.time_stamp = i;
state.state(RobotLocalization::StateMemberX) = i;
state.state(RobotLocalization::StateMemberY) = 0;
state.state(RobotLocalization::StateMemberZ) = 0;
state.state(RobotLocalization::StateMemberRoll) = 0;
state.state(RobotLocalization::StateMemberPitch) = 0;
state.state(RobotLocalization::StateMemberYaw) = 0;
state.state(RobotLocalization::StateMemberVx) = 1;
state.state(RobotLocalization::StateMemberVy) = 0;
state.state(RobotLocalization::StateMemberVz) = 0;
state.state(RobotLocalization::StateMemberVroll) = 0;
state.state(RobotLocalization::StateMemberVpitch) = 0;
state.state(RobotLocalization::StateMemberVyaw) = 0;
state.state(RobotLocalization::StateMemberAx) = 0;
state.state(RobotLocalization::StateMemberAy) = 0;
state.state(RobotLocalization::StateMemberAz) = 0;
states.push_back(state);
}
// Instantiate a robot localization estimator with a buffer capacity of 5
int buffer_capacity = 5;
Eigen::MatrixXd process_noise_covariance = Eigen::MatrixXd::Identity(RobotLocalization::STATE_SIZE,
RobotLocalization::STATE_SIZE);
RobotLocalization::RobotLocalizationEstimator estimator(buffer_capacity, RobotLocalization::FilterTypes::EKF,
process_noise_covariance);
RobotLocalization::EstimatorState state;
// Add the states in chronological order
for ( int i = 0; i < 6; i++ )
{
estimator.setState(states[i]);
// Check that the state is added correctly
estimator.getState(states[i].time_stamp, state);
EXPECT_EQ(state.time_stamp, states[i].time_stamp);
}
// We filled the buffer with more states that it can hold, so its size should now be equal to the capacity
EXPECT_EQ(static_cast<int>(estimator.getSize()), buffer_capacity);
// Clear the buffer and check if it's really empty afterwards
estimator.clearBuffer();
EXPECT_EQ(estimator.getSize(), 0u);
// Add states at time 1 through 3 inclusive to the buffer (buffer is not yet full)
for ( int i = 1; i < 4; i++ )
{
estimator.setState(states[i]);
}
// Now add a state at time 0, but let's change it a bit (set StateMemberY=12) so that we can inspect if it is
// correctly added to the buffer.
RobotLocalization::EstimatorState state_2 = states[0];
state_2.state(RobotLocalization::StateMemberY) = 12;
estimator.setState(state_2);
EXPECT_EQ(RobotLocalization::EstimatorResults::Exact,
estimator.getState(states[0].time_stamp, state));
// Check if the state is correctly added
EXPECT_EQ(state.state, state_2.state);
// Add some more states. State at t=0 should now be dropped, so we should get the prediction, which means y=0
for ( int i = 5; i < 8; i++ )
{
estimator.setState(states[i]);
}
EXPECT_EQ(RobotLocalization::EstimatorResults::ExtrapolationIntoPast,
estimator.getState(states[0].time_stamp, state));
EXPECT_EQ(states[0].state, state.state);
// Estimate a state that is not in the buffer, but can be determined by interpolation. The predicted state vector
// should be equal to the designed state at the requested time.
EXPECT_EQ(RobotLocalization::EstimatorResults::Interpolation,
estimator.getState(states[4].time_stamp, state));
EXPECT_EQ(states[4].state, state.state);
// Estimate a state that is not in the buffer, but can be determined by extrapolation into the future. The predicted
// state vector should be equal to the designed state at the requested time.
EXPECT_EQ(RobotLocalization::EstimatorResults::ExtrapolationIntoFuture,
estimator.getState(states[8].time_stamp, state));
EXPECT_EQ(states[8].state, state.state);
// Add missing state somewhere in the middle
estimator.setState(states[4]);
// Overwrite state at t=3 (oldest state now in the buffer) and check if it's correctly overwritten.
state_2 = states[3];
state_2.state(RobotLocalization::StateMemberVy) = -1.0;
estimator.setState(state_2);
EXPECT_EQ(RobotLocalization::EstimatorResults::Exact,
estimator.getState(states[3].time_stamp, state));
EXPECT_EQ(state_2.state, state.state);
// Add state that came too late
estimator.setState(states[0]);
// Check if getState needed to do extrapolation into the past
EXPECT_EQ(estimator.getState(states[0].time_stamp, state),
RobotLocalization::EstimatorResults::ExtrapolationIntoPast);
// Check state at t=0. This can only work correctly if the state at t=3 is
// overwritten and the state at zero is not in the buffer.
EXPECT_DOUBLE_EQ(3.0, state.state(RobotLocalization::StateMemberY));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_robot_localization_estimator");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

7
Localizations/Packages/robot_localization/test/test_robot_localization_estimator.test Normal file
View File

@@ -0,0 +1,7 @@
<!-- Launch file for ekf_test -->
<launch>
<test test-name="test_rle" pkg="robot_localization" type="test_robot_localization_estimator" clear_params="true" time-limit="100.0" />
</launch>

132
Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.cpp Normal file
View File

@@ -0,0 +1,132 @@
/*
* Copyright (c) 2016, TNO IVS, Helmond
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_robot_localization_listener.h"
#include "robot_localization/filter_common.h"
#include <string>
#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <gtest/gtest.h>
RobotLocalization::RosRobotLocalizationListener* g_listener;
TEST(LocalizationListenerTest, testGetStateOfBaseLink)
{
ros::spinOnce();
ros::Time time2(1001);
Eigen::VectorXd state(RobotLocalization::STATE_SIZE);
Eigen::MatrixXd covariance(RobotLocalization::STATE_SIZE, RobotLocalization::STATE_SIZE);
std::string base_frame("base_link");
g_listener->getState(time2, base_frame, state, covariance);
EXPECT_DOUBLE_EQ(1.0, state(RobotLocalization::StateMemberX));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberY));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberZ));
EXPECT_FLOAT_EQ(M_PI/4, state(RobotLocalization::StateMemberRoll));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberPitch));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberYaw));
EXPECT_DOUBLE_EQ(M_PI/4.0, state(RobotLocalization::StateMemberVroll));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberVpitch));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberVyaw));
}
TEST(LocalizationListenerTest, GetStateOfRelatedFrame)
{
ros::spinOnce();
Eigen::VectorXd state(RobotLocalization::STATE_SIZE);
Eigen::MatrixXd covariance(RobotLocalization::STATE_SIZE, RobotLocalization::STATE_SIZE);
ros::Time time1(1000);
ros::Time time2(1001);
std::string sensor_frame("sensor");
EXPECT_TRUE(g_listener->getState(time1, sensor_frame, state, covariance) );
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberX));
EXPECT_FLOAT_EQ(1.0, state(RobotLocalization::StateMemberY));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberZ));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberRoll));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberPitch));
EXPECT_FLOAT_EQ(M_PI/2, state(RobotLocalization::StateMemberYaw));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVx));
EXPECT_FLOAT_EQ(-1.0, state(RobotLocalization::StateMemberVy));
EXPECT_FLOAT_EQ(M_PI/4.0, state(RobotLocalization::StateMemberVz));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVroll));
EXPECT_FLOAT_EQ(-M_PI/4.0, state(RobotLocalization::StateMemberVpitch));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberVyaw));
EXPECT_TRUE(g_listener->getState(time2, sensor_frame, state, covariance));
EXPECT_FLOAT_EQ(1.0, state(RobotLocalization::StateMemberX));
EXPECT_FLOAT_EQ(sqrt(2)/2.0, state(RobotLocalization::StateMemberY));
EXPECT_FLOAT_EQ(sqrt(2)/2.0, state(RobotLocalization::StateMemberZ));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberRoll));
EXPECT_TRUE(1e-12 > fabs(-M_PI/4.0 - state(RobotLocalization::StateMemberPitch)));
EXPECT_FLOAT_EQ(M_PI/2, state(RobotLocalization::StateMemberYaw));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVx));
EXPECT_FLOAT_EQ(-1.0, state(RobotLocalization::StateMemberVy));
EXPECT_FLOAT_EQ(M_PI/4, state(RobotLocalization::StateMemberVz));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVroll));
EXPECT_FLOAT_EQ(-M_PI/4.0, state(RobotLocalization::StateMemberVpitch));
EXPECT_FLOAT_EQ(0, state(RobotLocalization::StateMemberVyaw));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_robot_localization_estimator");
g_listener = new RobotLocalization::RosRobotLocalizationListener();
testing::InitGoogleTest(&argc, argv);
int res = RUN_ALL_TESTS();
delete g_listener;
return res;
}

68
Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.test Normal file
View File

@@ -0,0 +1,68 @@
<!-- Launch file for ros_robot_localization_listener_test -->
<launch>
<node name="test_estimator" pkg="robot_localization" type="test_ros_robot_localization_listener_publisher" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/odom_input0"/>
<param name="odom1" value="/odom_input1"/>
<param name="odom2" value="/odom_input2"/>
<param name="pose0" value="/pose_input0"/>
<param name="pose1" value="/pose_input1"/>
<param name="twist0" value="/twist_input0"/>
<param name="imu0" value="/imu_input0"/>
<param name="imu1" value="/imu_input1"/>
<param name="imu2" value="/imu_input2"/>
<param name="imu3" value="/imu_input3"/>
<rosparam param="odom0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom2_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="pose0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="pose1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="imu1_config">[false, false, false, false, false, false, false, false, false, true, true, true, false, false, false]</rosparam>
<rosparam param="imu2_config">[false, false, false, false, false, false, false, false, false, false, false, false, true, true, true]</rosparam>
<rosparam param="imu3_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<param name="odom1_differential" value="true"/>
<param name="pose1_differential" value="true"/>
<param name="imu3_differential" value="true"/>
<param name="print_diagnostics" value="false"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
<test test-name="test_ros_robot_localization_listener" pkg="robot_localization" type="test_ros_robot_localization_listener" clear_params="true" time-limit="100.0">
<remap from="robot_localization" to="test_estimator" />
</test>
</launch>

117
Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener_publisher.cpp Normal file
View File

@@ -0,0 +1,117 @@
/*
* Copyright (c) 2016, TNO IVS, Helmond
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <geometry_msgs/AccelWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <string>
#include <ros/ros.h>
#include <tf2/utils.h>
#include <tf2_ros/static_transform_broadcaster.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_robot_localization_listener_publisher");
ros::NodeHandle nh;
ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odometry/filtered", 1);
ros::Publisher accel_pub = nh.advertise<geometry_msgs::AccelWithCovarianceStamped>("accel/filtered", 1);
tf2_ros::StaticTransformBroadcaster transform_broadcaster;
ros::Time end_time = ros::Time::now() + ros::Duration(10);
while (ros::ok() && ros::Time::now() < end_time)
{
ros::Time time1(1000);
double x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az;
x = y = z = roll = pitch = yaw = vy = vz = vroll = vpitch = vyaw = ax = ay = az = 0.0;
vx = 1.0;
vroll = M_PI/4.0;
tf2::Quaternion q;
q.setRPY(0, 0, 0);
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = time1;
odom_msg.header.frame_id = "map";
odom_msg.child_frame_id = "base_link";
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.position.y = z;
odom_msg.pose.pose.orientation.x = q.x();
odom_msg.pose.pose.orientation.y = q.y();
odom_msg.pose.pose.orientation.z = q.z();
odom_msg.pose.pose.orientation.w = q.w();
odom_msg.twist.twist.linear.x = vx;
odom_msg.twist.twist.linear.y = vy;
odom_msg.twist.twist.linear.z = vz;
odom_msg.twist.twist.angular.x = vroll;
odom_msg.twist.twist.angular.y = vpitch;
odom_msg.twist.twist.angular.z = vyaw;
geometry_msgs::AccelWithCovarianceStamped accel_msg;
accel_msg.header.stamp = time1;
accel_msg.header.frame_id = "base_link";
accel_msg.accel.accel.linear.x = ax;
accel_msg.accel.accel.linear.y = ay;
accel_msg.accel.accel.linear.z = az;
accel_msg.accel.accel.angular.x = 0;
accel_msg.accel.accel.angular.y = 0;
accel_msg.accel.accel.angular.z = 0;
odom_pub.publish(odom_msg);
accel_pub.publish(accel_msg);
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "base_link";
transformStamped.child_frame_id = "sensor";
transformStamped.transform.translation.x = 0.0;
transformStamped.transform.translation.y = 1.0;
transformStamped.transform.translation.z = 0.0;
{
tf2::Quaternion q;
q.setRPY(0, 0, M_PI/2);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
transform_broadcaster.sendTransform(transformStamped);
}
ros::Duration(0.1).sleep();
}
return 0;
}

141
Localizations/Packages/robot_localization/test/test_ukf.cpp Normal file
View File

@@ -0,0 +1,141 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <gtest/gtest.h>
#include <limits>
#include <vector>
using RobotLocalization::Ukf;
using RobotLocalization::RosUkf;
using RobotLocalization::STATE_SIZE;
class RosUkfPassThrough : public RosUkf
{
public:
explicit RosUkfPassThrough(std::vector<double> &args) : RosUkf(ros::NodeHandle(), ros::NodeHandle("~"), args)
{
}
Ukf &getFilter()
{
return filter_;
}
};
TEST(UkfTest, Measurements)
{
std::vector<double> args;
args.push_back(0.001);
args.push_back(0);
args.push_back(2);
RosUkfPassThrough ukf(args);
Eigen::MatrixXd initialCovar(15, 15);
initialCovar.setIdentity();
initialCovar *= 0.5;
ukf.getFilter().setEstimateErrorCovariance(initialCovar);
EXPECT_EQ(ukf.getFilter().getEstimateErrorCovariance(), initialCovar);
Eigen::VectorXd measurement(STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurement[i] = i * 0.01 * STATE_SIZE;
}
Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
measurementCovariance.setIdentity();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
std::vector<int> updateVector(STATE_SIZE, true);
// Ensure that measurements are being placed in the queue correctly
ros::Time time;
time.fromSec(1000);
ukf.enqueueMeasurement("odom0",
measurement,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ukf.integrateMeasurements(ros::Time(1001));
EXPECT_EQ(ukf.getFilter().getState(), measurement);
EXPECT_EQ(ukf.getFilter().getEstimateErrorCovariance(), measurementCovariance);
ukf.getFilter().setEstimateErrorCovariance(initialCovar);
// Now fuse another measurement and check the output.
// We know what the filter's state should be when
// this is complete, so we'll check the difference and
// make sure it's suitably small.
Eigen::VectorXd measurement2 = measurement;
measurement2 *= 2.0;
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
time.fromSec(1002);
ukf.enqueueMeasurement("odom0",
measurement2,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ukf.integrateMeasurements(ros::Time(1003));
measurement = measurement2.eval() - ukf.getFilter().getState();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
EXPECT_LT(::fabs(measurement[i]), 0.001);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ukf");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

7
Localizations/Packages/robot_localization/test/test_ukf.test Normal file
View File

@@ -0,0 +1,7 @@
<!-- Launch file for test_ukf -->
<launch>
<test test-name="test_ukf" pkg="robot_localization" type="test_ukf" clear_params="true" time-limit="100.0" />
</launch>

110
Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag1.test Normal file
View File

@@ -0,0 +1,110 @@
<!-- Launch file for test_ukf_localization_node_bag1 -->
<!-- In this run, we ran a Clearpath Husky with a slightly broken Microstrain 3DM-GX2 IMU
around a parking lot. The IMU was intentionally rotated +90 degrees about the X axis,
then rotated (extrinsically) -90 degrees about the Z axis. The IMU did not report 0
when facing east, however, but when facing north. -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test1.bag --clock -d 5" required="true"/>
<node name="test_ukf_localization_node_bag1_ukf" pkg="robot_localization" type="ukf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ukf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
<param name="alpha" value="0.001"/>
<param name="kappa" value="0"/>
<param name="beta" value="2"/>
</node>
<test test-name="test_ukf_localization_node_bag1_pose" pkg="robot_localization" type="test_ukf_localization_node_bag1" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-39.7333"/>
<param name="final_y" value="-76.9820"/>
<param name="final_z" value="-2.4427"/>
<param name="tolerance" value="1.2910"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

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