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

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.