├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── doc ├── .templates │ └── full_globaltoc.html ├── CHANGELOG.rst ├── Makefile ├── conf.py ├── configuring_robot_localization.rst ├── images │ ├── figure2.png │ ├── navsat_transform_workflow.png │ ├── navsat_transform_workflow.tex │ └── rl_small.png ├── index.rst ├── integrating_gps.rst ├── manifest.yaml ├── migrating_from_robot_pose_ekf.rst ├── navsat_transform_node.rst ├── preparing_sensor_data.rst ├── robot_localization_ias13_revised.pdf └── state_estimation_nodes.rst ├── include └── robot_localization │ ├── ekf.hpp │ ├── filter_base.hpp │ ├── filter_common.hpp │ ├── filter_state.hpp │ ├── filter_utilities.hpp │ ├── measurement.hpp │ ├── navsat_conversions.hpp │ ├── navsat_transform.hpp │ ├── robot_localization_estimator.hpp │ ├── ros_filter.hpp │ ├── ros_filter_types.hpp │ ├── ros_filter_utilities.hpp │ ├── ros_robot_localization_listener.hpp │ └── ukf.hpp ├── launch ├── dual_ekf_navsat_example.launch.py ├── ekf.launch.py ├── navsat_transform.launch.py └── ukf.launch.py ├── package.xml ├── params ├── dual_ekf_navsat_example.yaml ├── ekf.yaml ├── navsat_transform.yaml └── ukf.yaml ├── rosdoc.yaml ├── src ├── ekf.cpp ├── ekf_node.cpp ├── filter_base.cpp ├── filter_utilities.cpp ├── navsat_transform.cpp ├── navsat_transform_node.cpp ├── robot_localization_estimator.cpp ├── robot_localization_listener_node.cpp ├── ros_filter.cpp ├── ros_filter_utilities.cpp ├── ros_robot_localization_listener.cpp ├── ukf.cpp └── ukf_node.cpp ├── srv ├── FromLL.srv ├── GetState.srv ├── SetDatum.srv ├── SetPose.srv ├── ToLL.srv └── ToggleFilterProcessing.srv └── test ├── test1.bag ├── test2.bag ├── test3.bag ├── test_ekf.cpp ├── test_ekf_localization_node_bag1.launch.py ├── test_ekf_localization_node_bag1.sh ├── test_ekf_localization_node_bag1.yaml ├── test_ekf_localization_node_bag2.launch.py ├── test_ekf_localization_node_bag2.sh ├── test_ekf_localization_node_bag2.yaml ├── test_ekf_localization_node_bag3.launch.py ├── test_ekf_localization_node_bag3.sh ├── test_ekf_localization_node_bag3.yaml ├── test_ekf_localization_node_interfaces.cpp ├── test_ekf_localization_node_interfaces.launch.py ├── test_ekf_localization_node_interfaces.yaml ├── test_filter_base.cpp ├── test_filter_base_diagnostics_timestamps.cpp ├── test_filter_base_diagnostics_timestamps.launch.py ├── test_filter_base_diagnostics_timestamps.yaml ├── test_localization_node_bag_pose_tester.cpp ├── test_robot_localization_estimator.cpp ├── test_robot_localization_estimator.launch.py ├── test_ros_robot_localization_listener.cpp ├── test_ros_robot_localization_listener.launch.py ├── test_ros_robot_localization_listener.yaml ├── test_ros_robot_localization_listener_publisher.cpp ├── test_ukf.cpp ├── test_ukf_localization_node_bag1.launch.py ├── test_ukf_localization_node_bag1.sh ├── test_ukf_localization_node_bag1.yaml ├── test_ukf_localization_node_bag2.launch.py ├── test_ukf_localization_node_bag2.sh ├── test_ukf_localization_node_bag2.yaml ├── test_ukf_localization_node_bag3.launch.py ├── test_ukf_localization_node_bag3.sh ├── test_ukf_localization_node_bag3.yaml ├── test_ukf_localization_node_interfaces.cpp ├── test_ukf_localization_node_interfaces.launch.py └── test_ukf_localization_node_interfaces.yaml /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | install 3 | log 4 | doc/html 5 | *.cproject 6 | *.project 7 | *.settings 8 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: bionic 2 | sudo: enabled 3 | language: generic 4 | 5 | env: 6 | global: 7 | - ROS2_WS_ROBOT_LOCALIZATION=~/ros2_ws_robot_localization 8 | - ROS2_WS_ROBOT_LOCALIZATION_SRC=${ROS2_WS_ROBOT_LOCALIZATION}/src 9 | 10 | # command to install dependencies 11 | install: 12 | - sudo apt update && sudo apt install curl 13 | - curl http://repo.ros2.org/repos.key | sudo apt-key add - 14 | - sudo sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' 15 | - export ROS_DISTRO=bouncy 16 | - sudo apt update && sudo apt install -y python3-rosdep ros-$ROS_DISTRO-ros-base 17 | - sudo apt install python3-argcomplete 18 | - sudo apt install python3-colcon-common-extensions 19 | - sudo apt install build-essential 20 | - source /opt/ros/bouncy/setup.bash 21 | - source ~/${ROS2_WS_ROBOT_LOCALIZATION}/install/setup.bash 22 | - sudo rosdep init 23 | - rosdep update 24 | - sudo apt install python3-pip 25 | - sudo pip3 install argcomplete setuptools pytest pep257 flake8 26 | - sudo -H python3 -m pip install -U setuptools 27 | 28 | 29 | # command to run tests 30 | script: 31 | # - mkdir -p $ROS2_WS_ROBOT_LOCALIZATION 32 | - mkdir -p $ROS2_WS_ROBOT_LOCALIZATION_SRC 33 | - ln -s $TRAVIS_BUILD_DIR $ROS2_WS_ROBOT_LOCALIZATION_SRC 34 | - cd $ROS2_WS_ROBOT_LOCALIZATION 35 | - colcon build 36 | # - colcon test 37 | 38 | branches: 39 | only: 40 | - ros2 41 | 42 | notifications: 43 | email: false 44 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | All code within the robot_localization package that was developed by Charles 2 | River Analytics is released under the BSD license: 3 | 4 | Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the {organization} nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | 32 | ========================================================================== 33 | 34 | The robot_localization package also makes use of code written by The University 35 | of Texas at Austin. This code is released under a modified BSD license: 36 | 37 | Copyright (C) 2010 Austin Robot Technology, and others 38 | All rights reserved. 39 | 40 | Redistribution and use in source and binary forms, with or without 41 | modification, are permitted provided that the following conditions 42 | are met: 43 | 44 | * Redistributions of source code must retain the above copyright 45 | notice, this list of conditions and the following disclaimer. 46 | 47 | * Redistributions in binary form must reproduce the above 48 | copyright notice, this list of conditions and the following 49 | disclaimer in the documentation and/or other materials provided 50 | with the distribution. 51 | 52 | * Neither the names of the University of Texas at Austin, nor 53 | Austin Robot Technology, nor the names of other contributors may 54 | be used to endorse or promote products derived from this 55 | software without specific prior written permission. 56 | 57 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 58 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 59 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 60 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 61 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 62 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 63 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 64 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 65 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 66 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 67 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 68 | POSSIBILITY OF SUCH DAMAGE. 69 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | robot_localization 2 | ================== 3 | 4 | robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. 5 | 6 | Please see documentation here: http://wiki.ros.org/robot_localization 7 | -------------------------------------------------------------------------------- /doc/.templates/full_globaltoc.html: -------------------------------------------------------------------------------- 1 |

{{ _('Table Of Contents') }}

2 | {{ toctree(includehidden=True) }} 3 | 4 |

{{ _('Further Documentation') }}

5 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /doc/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ../CHANGELOG.rst -------------------------------------------------------------------------------- /doc/conf.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | 4 | import catkin_pkg.package 5 | catkin_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 6 | catkin_package = catkin_pkg.package.parse_package(os.path.join(catkin_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) 7 | 8 | extensions = [ 9 | 'sphinx.ext.autodoc', 10 | 'sphinx.ext.doctest', 11 | 'sphinx.ext.intersphinx', 12 | 'sphinx.ext.todo', 13 | 'sphinx.ext.coverage', 14 | 'sphinx.ext.mathjax', 15 | ] 16 | 17 | templates_path = ['.templates'] 18 | source_suffix = '.rst' 19 | master_doc = 'index' 20 | 21 | project = u'robot_localization' 22 | copyright = u'2016, Tom Moore' 23 | author = u'Tom Moore' 24 | version = catkin_package.version 25 | release = catkin_package.version 26 | 27 | language = None 28 | exclude_patterns = ['.build'] 29 | pygments_style = 'sphinx' 30 | todo_include_todos = True 31 | 32 | html_theme = 'nature' 33 | 34 | html_theme_options = { 35 | "sidebarwidth": "350" 36 | } 37 | 38 | # The name of an image file (relative to this directory) to place at the top 39 | # of the sidebar. 40 | html_logo = 'images/rl_small.png' 41 | 42 | # The name of an image file (relative to this directory) to use as a favicon of 43 | # the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 44 | # pixels large. 45 | #html_favicon = None 46 | 47 | # Add any paths that contain custom static files (such as style sheets) here, 48 | # relative to this directory. They are copied after the builtin static files, 49 | # so a file named "default.css" will overwrite the builtin "default.css". 50 | #html_static_path = ['.static'] 51 | 52 | # Add any extra paths that contain custom files (such as robots.txt or 53 | # .htaccess) here, relative to this directory. These files are copied 54 | # directly to the root of the documentation. 55 | #html_extra_path = [] 56 | 57 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 58 | # using the given strftime format. 59 | #html_last_updated_fmt = '%b %d, %Y' 60 | 61 | # If true, SmartyPants will be used to convert quotes and dashes to 62 | # typographically correct entities. 63 | #html_use_smartypants = True 64 | 65 | # Custom sidebar templates, maps document names to template names. 66 | html_sidebars = { '**': ['full_globaltoc.html', 'sourcelink.html', 'searchbox.html'], } 67 | 68 | # Additional templates that should be rendered to pages, maps page names to 69 | # template names. 70 | #html_additional_pages = {} 71 | 72 | # If false, no module index is generated. 73 | #html_domain_indices = True 74 | 75 | # If false, no index is generated. 76 | #html_use_index = True 77 | 78 | # If true, the index is split into individual pages for each letter. 79 | #html_split_index = False 80 | 81 | # If true, links to the reST sources are added to the pages. 82 | #html_show_sourcelink = True 83 | 84 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 85 | #html_show_sphinx = True 86 | 87 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 88 | #html_show_copyright = True 89 | 90 | # If true, an OpenSearch description file will be output, and all pages will 91 | # contain a tag referring to it. The value of this option must be the 92 | # base URL from which the finished HTML is served. 93 | #html_use_opensearch = '' 94 | 95 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 96 | #html_file_suffix = None 97 | 98 | # Language to be used for generating the HTML full-text search index. 99 | # Sphinx supports the following languages: 100 | # 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' 101 | # 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' 102 | #html_search_language = 'en' 103 | 104 | # A dictionary with options for the search language support, empty by default. 105 | # Now only 'ja' uses this config value 106 | #html_search_options = {'type': 'default'} 107 | 108 | # The name of a javascript file (relative to the configuration directory) that 109 | # implements a search results scorer. If empty, the default will be used. 110 | #html_search_scorer = 'scorer.js' 111 | 112 | # Output file base name for HTML help builder. 113 | htmlhelp_basename = 'robot_localizationdoc' 114 | 115 | # -- Options for LaTeX output --------------------------------------------- 116 | 117 | latex_elements = { 118 | # The paper size ('letterpaper' or 'a4paper'). 119 | #'papersize': 'letterpaper', 120 | 121 | # The font size ('10pt', '11pt' or '12pt'). 122 | #'pointsize': '10pt', 123 | 124 | # Additional stuff for the LaTeX preamble. 125 | #'preamble': '', 126 | 127 | # Latex figure (float) alignment 128 | #'figure_align': 'htbp', 129 | } 130 | 131 | # Grouping the document tree into LaTeX files. List of tuples 132 | # (source start file, target name, title, 133 | # author, documentclass [howto, manual, or own class]). 134 | latex_documents = [ 135 | (master_doc, 'robot_localization.tex', u'robot\\_localization Documentation', 136 | u'Tom Moore', 'manual'), 137 | ] 138 | 139 | # The name of an image file (relative to this directory) to place at the top of 140 | # the title page. 141 | #latex_logo = None 142 | 143 | # For "manual" documents, if this is true, then toplevel headings are parts, 144 | # not chapters. 145 | #latex_use_parts = False 146 | 147 | # If true, show page references after internal links. 148 | #latex_show_pagerefs = False 149 | 150 | # If true, show URL addresses after external links. 151 | #latex_show_urls = False 152 | 153 | # Documents to append as an appendix to all manuals. 154 | #latex_appendices = [] 155 | 156 | # If false, no module index is generated. 157 | #latex_domain_indices = True 158 | 159 | 160 | # -- Options for manual page output --------------------------------------- 161 | 162 | # One entry per manual page. List of tuples 163 | # (source start file, name, description, authors, manual section). 164 | man_pages = [ 165 | (master_doc, 'robot_localization', u'robot_localization Documentation', 166 | [author], 1) 167 | ] 168 | 169 | # If true, show URL addresses after external links. 170 | #man_show_urls = False 171 | 172 | 173 | # -- Options for Texinfo output ------------------------------------------- 174 | 175 | # Grouping the document tree into Texinfo files. List of tuples 176 | # (source start file, target name, title, author, 177 | # dir menu entry, description, category) 178 | texinfo_documents = [ 179 | (master_doc, 'robot_localization', u'robot_localization Documentation', 180 | author, 'robot_localization', 'One line description of project.', 181 | 'Miscellaneous'), 182 | ] 183 | 184 | # Documents to append as an appendix to all manuals. 185 | #texinfo_appendices = [] 186 | 187 | # If false, no module index is generated. 188 | #texinfo_domain_indices = True 189 | 190 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 191 | #texinfo_show_urls = 'footnote' 192 | 193 | # If true, do not generate a @detailmenu in the "Top" node's menu. 194 | #texinfo_no_detailmenu = False 195 | 196 | # Example configuration for intersphinx: refer to the Python standard library. 197 | intersphinx_mapping = {'https://docs.python.org/': None} 198 | -------------------------------------------------------------------------------- /doc/images/figure2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/automaticaddison/robot_localization/f77f5a6e5f986a6928c9eea40b5d9c6e600b42a9/doc/images/figure2.png -------------------------------------------------------------------------------- /doc/images/navsat_transform_workflow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/automaticaddison/robot_localization/f77f5a6e5f986a6928c9eea40b5d9c6e600b42a9/doc/images/navsat_transform_workflow.png -------------------------------------------------------------------------------- /doc/images/navsat_transform_workflow.tex: -------------------------------------------------------------------------------- 1 | \documentclass{standalone} 2 | \usepackage{tikz} 3 | \usetikzlibrary{positioning} 4 | 5 | \begin{document} 6 | 7 | \sffamily 8 | 9 | \begin{tikzpicture}[>=stealth, 10 | node distance = 3cm, 11 | box/.style={shape=rectangle,draw,rounded corners},] 12 | % Nodes 13 | \node (wheel) [box] {Wheel Odometry}; 14 | \node (imu) [box, right =of wheel]{IMU}; 15 | \node (ekfLocal) [box, below =of wheel]{EKF Local}; 16 | \node (ekfGlobal) [box, below =of imu]{EKF Global}; 17 | \node (navsat) [box, right =7cm of ekfGlobal]{navsat\_transform}; 18 | \node (gps) [box, above =of navsat]{GPS}; 19 | % Coordinates for edges pointing to empty space 20 | \node (gpsF) [right =of navsat]{}; 21 | \node (tfLocal) [below =of ekfLocal]{}; 22 | \node (odomLocal) [left =of ekfLocal]{}; 23 | \node (tfGlobal) [below =of ekfGlobal]{}; 24 | % Edges 25 | \draw[->] (wheel.270) -- (ekfLocal.90); 26 | \draw[->] (wheel.315) -- (ekfGlobal.135) 27 | node [fill=white, pos=0.2, align=center] {\ttfamily"/wheel/odometry"\\nav\_msgs/Odometry}; 28 | \draw[->] (imu.225) -- (ekfLocal.45); 29 | \draw[->] (imu.315) -- (navsat.135); 30 | \draw[->] (imu.270) -- (ekfGlobal.90) 31 | node [fill=white, pos=0.2, align=center] {\ttfamily"/imu/data"\\sensor\_msgs/Imu}; 32 | \draw[->] (gps.270) -- (navsat.90) 33 | node [fill=white, pos=0.2, align=center] {\ttfamily"/gps/fix"\\sensor\_msgs/NavSatFix}; 34 | \draw[->, transform canvas={yshift=1mm}] (ekfGlobal) -- (navsat) 35 | node [fill=white, above=1mm, pos=0.5, align=center] {\ttfamily"/odometry/filtered/global"\\nav\_msgs/Odometry}; 36 | \draw[->, transform canvas={yshift=-1mm}] (navsat) -- (ekfGlobal) 37 | node [fill=white, below=1mm, pos=0.5, align=center] {\ttfamily"/odometry/gps"\\nav\_msgs/Odometry}; 38 | % Outputs not cycled back into the graph 39 | \draw[->, dashed] (navsat) -- (gpsF) 40 | node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/gps/filtered"\\sensor\_msgs/NavSatFix}; 41 | \draw[->, dashed] (ekfLocal) -- (tfLocal) 42 | node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" odom -> base\\tf2\_msgs/TFMessage}; 43 | \draw[->, dashed] (ekfLocal) -- (odomLocal) 44 | node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/odometry/filtered/local"\\nav\_msgs/Odometry}; 45 | \draw[->, dashed] (ekfGlobal) -- (tfGlobal) 46 | node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" map -> odom\\tf2\_msgs/TFMessage}; 47 | \end{tikzpicture} 48 | 49 | \end{document} 50 | -------------------------------------------------------------------------------- /doc/images/rl_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/automaticaddison/robot_localization/f77f5a6e5f986a6928c9eea40b5d9c6e600b42a9/doc/images/rl_small.png -------------------------------------------------------------------------------- /doc/index.rst: -------------------------------------------------------------------------------- 1 | .. _index: 2 | 3 | robot_localization wiki 4 | *********************** 5 | 6 | ``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. 7 | 8 | .. toctree:: 9 | :hidden: 10 | 11 | state_estimation_nodes 12 | navsat_transform_node 13 | preparing_sensor_data 14 | configuring_robot_localization 15 | migrating_from_robot_pose_ekf 16 | integrating_gps 17 | CHANGELOG 18 | 19 | Features 20 | ======== 21 | 22 | All the state estimation nodes in ``robot_localization`` share common features, namely: 23 | 24 | * 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. 25 | * Support for multiple ROS message types. All state estimation nodes in ``robot_localization`` can take in `nav_msgs/Odometry `_, `sensor_msgs/Imu `_, `geometry_msgs/PoseWithCovarianceStamped `_, or `geometry_msgs/TwistWithCovarianceStamped `_ messages. 26 | * 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. 27 | * 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. 28 | 29 | 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})`. 30 | 31 | Other Resources 32 | =============== 33 | 34 | If you're new to ``robot_localization``, check out the `2015 ROSCon talk `_ for some pointers on getting started. 35 | 36 | Further details can be found in :download:`this paper `: 37 | 38 | .. code-block:: none 39 | 40 | @inproceedings{MooreStouchKeneralizedEkf2014, 41 | author = {T. Moore and D. Stouch}, 42 | title = {A Generalized Extended Kalman Filter Implementation for the Robot Operating System}, 43 | year = {2014}, 44 | month = {July}, 45 | booktitle = {Proceedings of the 13th International Conference on Intelligent Autonomous Systems (IAS-13)}, 46 | publisher = {Springer} 47 | } 48 | 49 | Indices and tables 50 | ================== 51 | 52 | * :ref:`genindex` 53 | * :ref:`search` 54 | 55 | -------------------------------------------------------------------------------- /doc/manifest.yaml: -------------------------------------------------------------------------------- 1 | actions: [] 2 | authors: Tom Moore 3 | brief: '' 4 | bugtracker: '' 5 | depends: 6 | - catkin 7 | - eigen 8 | - diagnostic_updater 9 | - cmake_modules 10 | - tf2 11 | - nav_msgs 12 | - roscpp 13 | - rostest 14 | - tf2_ros 15 | - message_generation 16 | - message_filters 17 | - tf2_geometry_msgs 18 | - sensor_msgs 19 | - message_runtime 20 | - std_msgs 21 | - roslint 22 | - rosunit 23 | - diagnostic_msgs 24 | - geographic_msgs 25 | - xmlrpcpp 26 | - python-catkin-pkg 27 | - geometry_msgs 28 | - rosbag 29 | description: The robot_localization package provides nonlinear state estimation through 30 | sensor fusion of an abritrary number of sensors. 31 | license: BSD 32 | maintainers: Tom Moore 33 | msgs: [] 34 | package_type: package 35 | repo_url: '' 36 | srvs: 37 | - SetPose 38 | - SetDatum 39 | url: http://ros.org/wiki/robot_localization 40 | -------------------------------------------------------------------------------- /doc/migrating_from_robot_pose_ekf.rst: -------------------------------------------------------------------------------- 1 | .. _migrating_from_robot_pose_ekf: 2 | 3 | Migrating from robot_pose_ekf 4 | ############################# 5 | 6 | Migration from ``robot_pose_ekf`` is fairly straightforward. This page is meant to highlight relevant differences between the packages to facilitate rapid transitions. 7 | 8 | Covariances in Source Messages 9 | ============================== 10 | 11 | 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). 12 | 13 | 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. 14 | 15 | The ''differential'' Parameter 16 | ============================== 17 | 18 | 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. 19 | 20 | This situation can be avoided using three different methods for the ``robot_localization`` state estimation nodes: 21 | 22 | 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. 23 | 24 | 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. 25 | 26 | 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. 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /doc/navsat_transform_node.rst: -------------------------------------------------------------------------------- 1 | navsat_transform_node 2 | ********************* 3 | 4 | ``navsat_transform_node`` takes as input a `nav_msgs/Odometry `_ message (usually the output of ``ekf_localization_node`` or ``ukf_localization_node``), a `sensor_msgs/Imu `_ containing an accurate estimate of your robot's heading, and a `sensor_msgs/NavSatFix `_ 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. 5 | 6 | .. 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. 7 | 8 | Parameters 9 | ========== 10 | 11 | ~frequency 12 | ^^^^^^^^^^ 13 | The real-valued frequency, in Hz, at which ``navsat_transform_node`` checks for new `sensor_msgs/NavSatFix `_ messages, and publishes filtered `sensor_msgs/NavSatFix `_ when ``publish_filtered_gps`` is set to *true*. 14 | 15 | ~delay 16 | ^^^^^^ 17 | The time, in seconds, to wait before calculating the transform from GPS coordinates to your robot's world frame. 18 | 19 | ~magnetic_declination_radians 20 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 21 | Enter the magnetic declination for your location. If you don't know it, see `http://www.ngdc.noaa.gov/geomag-web `_ (make sure to convert the value to radians). This parameter is needed if your IMU provides its orientation with respect to the magnetic north. 22 | 23 | ~yaw_offset 24 | ^^^^^^^^^^^ 25 | 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. 26 | 27 | ~zero_altitude 28 | ^^^^^^^^^^^^^^ 29 | If this is *true*, the `nav_msgs/Odometry `_ message produced by this node has its pose Z value set to 0. 30 | 31 | ~publish_filtered_gps 32 | ^^^^^^^^^^^^^^^^^^^^^ 33 | 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 `_ message on the ``/gps/filtered`` topic. 34 | 35 | ~broadcast_utm_transform 36 | ^^^^^^^^^^^^^^^^^^^^^^^^ 37 | 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. 38 | 39 | ~use_odometry_yaw 40 | ^^^^^^^^^^^^^^^^^ 41 | 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*. 42 | 43 | ~wait_for_datum 44 | ^^^^^^^^^^^^^^^ 45 | If *true*, ``navsat_transform_node`` will wait to get a datum from either: 46 | 47 | * The ``datum`` parameter 48 | * The ``set_datum`` service 49 | 50 | ~broadcast_utm_transform_as_parent_frame 51 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 52 | If *true*, ``navsat_transform_node`` will publish the utm->world_frame transform instead of the world_frame->utm transform. 53 | Note that for the transform to be published ``broadcast_utm_transform`` also has to be set to *true*. 54 | 55 | ~transform_timeout 56 | ^^^^^^^^^^^^^^^^^^ 57 | 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. 58 | 59 | Subscribed Topics 60 | ================= 61 | * ``imu/data`` A `sensor_msgs/Imu `_ message with orientation data 62 | 63 | * ``odometry/filtered`` A `nav_msgs/Odometry `_ 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. 64 | 65 | * ``gps/fix`` A `sensor_msgs/NavSatFix `_ message containing your robot's GPS coordinates 66 | 67 | Published Topics 68 | ================ 69 | * ``odometry/gps`` A `nav_msgs/Odometry `_ 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. 70 | 71 | * ``gps/filtered`` (optional) A `sensor_msgs/NavSatFix `_ message containing your robot's world frame position, transformed into GPS coordinates 72 | 73 | Published Transforms 74 | ==================== 75 | * ``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. 76 | -------------------------------------------------------------------------------- /doc/robot_localization_ias13_revised.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/automaticaddison/robot_localization/f77f5a6e5f986a6928c9eea40b5d9c6e600b42a9/doc/robot_localization_ias13_revised.pdf -------------------------------------------------------------------------------- /include/robot_localization/ekf.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * Copyright (c) 2017, Locus Robotics, Inc. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #ifndef ROBOT_LOCALIZATION__EKF_HPP_ 35 | #define ROBOT_LOCALIZATION__EKF_HPP_ 36 | 37 | #include 38 | #include 39 | 40 | namespace robot_localization 41 | { 42 | 43 | /** 44 | * @brief Extended Kalman filter class 45 | * 46 | * Implementation of an extended Kalman filter (EKF). This class derives from 47 | * FilterBase and overrides the predict() and correct() methods in keeping with 48 | * the discrete time EKF algorithm. 49 | */ 50 | class Ekf : public FilterBase 51 | { 52 | public: 53 | /** 54 | * @brief Constructor for the Ekf class 55 | */ 56 | Ekf(); 57 | 58 | /** 59 | * @brief Destructor for the Ekf class 60 | */ 61 | ~Ekf(); 62 | 63 | /** 64 | * @brief Carries out the correct step in the predict/update cycle. 65 | * 66 | * @param[in] measurement - The measurement to fuse with our estimate 67 | */ 68 | void correct(const Measurement & measurement) override; 69 | 70 | /** 71 | * @brief Carries out the predict step in the predict/update cycle. 72 | * 73 | * Projects the state and error matrices forward using a model of the 74 | * vehicle's motion. 75 | * 76 | * @param[in] reference_time - The time at which the prediction is being made 77 | * @param[in] delta - The time step over which to predict. 78 | */ 79 | void predict( 80 | const rclcpp::Time & reference_time, 81 | const rclcpp::Duration & delta) override; 82 | }; 83 | 84 | } // namespace robot_localization 85 | 86 | #endif // ROBOT_LOCALIZATION__EKF_HPP_ 87 | -------------------------------------------------------------------------------- /include/robot_localization/filter_common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef ROBOT_LOCALIZATION__FILTER_COMMON_HPP_ 34 | #define ROBOT_LOCALIZATION__FILTER_COMMON_HPP_ 35 | 36 | namespace robot_localization 37 | { 38 | 39 | /** 40 | * @brief Enumeration that defines the state vector 41 | */ 42 | enum StateMembers 43 | { 44 | StateMemberX = 0, 45 | StateMemberY, 46 | StateMemberZ, 47 | StateMemberRoll, 48 | StateMemberPitch, 49 | StateMemberYaw, 50 | StateMemberVx, 51 | StateMemberVy, 52 | StateMemberVz, 53 | StateMemberVroll, 54 | StateMemberVpitch, 55 | StateMemberVyaw, 56 | StateMemberAx, 57 | StateMemberAy, 58 | StateMemberAz 59 | }; 60 | 61 | /** 62 | * @brief Enumeration that defines the control vector 63 | */ 64 | enum ControlMembers 65 | { 66 | ControlMemberVx, 67 | ControlMemberVy, 68 | ControlMemberVz, 69 | ControlMemberVroll, 70 | ControlMemberVpitch, 71 | ControlMemberVyaw 72 | }; 73 | 74 | /** 75 | * @brief Global constants that define our state 76 | * vector size and offsets to groups of values 77 | * within that state. 78 | */ 79 | const int STATE_SIZE = 15; 80 | const int POSITION_OFFSET = StateMemberX; 81 | const int ORIENTATION_OFFSET = StateMemberRoll; 82 | const int POSITION_V_OFFSET = StateMemberVx; 83 | const int ORIENTATION_V_OFFSET = StateMemberVroll; 84 | const int POSITION_A_OFFSET = StateMemberAx; 85 | 86 | /** 87 | * @brief Pose and twist messages each contain six variables 88 | */ 89 | const int POSE_SIZE = 6; 90 | const int TWIST_SIZE = 6; 91 | const int POSITION_SIZE = 3; 92 | const int ORIENTATION_SIZE = 3; 93 | const int LINEAR_VELOCITY_SIZE = 3; 94 | const int ACCELERATION_SIZE = 3; 95 | 96 | /** 97 | * @brief Common constants 98 | */ 99 | const double PI = 3.141592653589793; 100 | const double TAU = 6.283185307179587; 101 | 102 | } // namespace robot_localization 103 | 104 | #endif // ROBOT_LOCALIZATION__FILTER_COMMON_HPP_ 105 | -------------------------------------------------------------------------------- /include/robot_localization/filter_state.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * Copyright (c) 2017, Locus Robotics, Inc. 4 | * Copyright (c) 2019, Steve Macenski 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROBOT_LOCALIZATION__FILTER_STATE_HPP_ 36 | #define ROBOT_LOCALIZATION__FILTER_STATE_HPP_ 37 | 38 | #include 39 | #include "Eigen/Dense" 40 | #include "rclcpp/duration.hpp" 41 | #include "rclcpp/macros.hpp" 42 | #include "rclcpp/time.hpp" 43 | 44 | namespace robot_localization 45 | { 46 | 47 | /** 48 | * @brief Structure used for storing and comparing filter states 49 | * 50 | * This structure is useful when higher-level classes need to remember filter 51 | * history. Measurement units are assumed to be in meters and radians. Times are 52 | * real-valued and measured in seconds. 53 | */ 54 | struct FilterState 55 | { 56 | FilterState() 57 | : state_(), estimate_error_covariance_(), latest_control_(), 58 | last_measurement_time_(0.0), latest_control_time_(0) 59 | { 60 | } 61 | 62 | // The filter state vector 63 | Eigen::VectorXd state_; 64 | 65 | // The filter error covariance matrix 66 | Eigen::MatrixXd estimate_error_covariance_; 67 | 68 | // The most recent control vector 69 | Eigen::VectorXd latest_control_; 70 | 71 | // The time stamp of the most recent measurement for the filter 72 | rclcpp::Time last_measurement_time_; 73 | 74 | // The time stamp of the most recent control term 75 | rclcpp::Time latest_control_time_; 76 | 77 | // We want the queue to be sorted from latest to earliest timestamps. 78 | bool operator()(const FilterState & a, const FilterState & b) 79 | { 80 | return a.last_measurement_time_ < b.last_measurement_time_; 81 | } 82 | }; 83 | using FilterStatePtr = std::shared_ptr; 84 | 85 | } // namespace robot_localization 86 | 87 | #endif // ROBOT_LOCALIZATION__FILTER_STATE_HPP_ 88 | -------------------------------------------------------------------------------- /include/robot_localization/filter_utilities.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef ROBOT_LOCALIZATION__FILTER_UTILITIES_HPP_ 34 | #define ROBOT_LOCALIZATION__FILTER_UTILITIES_HPP_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #define FB_DEBUG(msg) \ 47 | if (getDebug()) { \ 48 | *debug_stream_ << msg; \ 49 | } 50 | 51 | // Handy methods for debug output 52 | std::ostream & operator<<(std::ostream & os, const Eigen::MatrixXd & mat); 53 | std::ostream & operator<<(std::ostream & os, const Eigen::VectorXd & vec); 54 | std::ostream & operator<<(std::ostream & os, const std::vector & vec); 55 | std::ostream & operator<<(std::ostream & os, const std::vector & vec); 56 | 57 | namespace robot_localization 58 | { 59 | namespace filter_utilities 60 | { 61 | 62 | /** 63 | * @brief Utility method keeping RPY angles in the range [-pi, pi] 64 | * @param[in] rotation - The rotation to bind 65 | * @return the bounded value 66 | */ 67 | inline double clampRotation(double rotation) 68 | { 69 | while (rotation > PI) { 70 | rotation -= TAU; 71 | } 72 | 73 | while (rotation < -PI) { 74 | rotation += TAU; 75 | } 76 | 77 | return rotation; 78 | } 79 | 80 | /** 81 | * @brief Utility method for appending tf2 prefixes cleanly 82 | * @param[in] tf_prefix - the tf2 prefix to append 83 | * @param[in, out] frame_id - the resulting frame_id value 84 | */ 85 | inline void appendPrefix(std::string tf_prefix, std::string & frame_id) 86 | { 87 | // Strip all leading slashes for tf2 compliance 88 | if (!frame_id.empty() && frame_id.at(0) == '/') { 89 | frame_id = frame_id.substr(1); 90 | } 91 | 92 | if (!tf_prefix.empty() && tf_prefix.at(0) == '/') { 93 | tf_prefix = tf_prefix.substr(1); 94 | } 95 | 96 | // If we do have a tf prefix, then put a slash in between 97 | if (!tf_prefix.empty()) { 98 | frame_id = tf_prefix + "/" + frame_id; 99 | } 100 | } 101 | 102 | inline double nanosecToSec(const rcl_time_point_value_t nanoseconds) 103 | { 104 | return static_cast(nanoseconds) * 1e-9; 105 | } 106 | 107 | inline int secToNanosec(const double seconds) 108 | { 109 | return static_cast(seconds * 1e9); 110 | } 111 | 112 | inline double toSec(const rclcpp::Duration & duration) 113 | { 114 | return nanosecToSec(duration.nanoseconds()); 115 | } 116 | 117 | inline double toSec(const rclcpp::Time & time) 118 | { 119 | return nanosecToSec(time.nanoseconds()); 120 | } 121 | 122 | inline double toSec(const std_msgs::msg::Header::_stamp_type & stamp) 123 | { 124 | return static_cast(stamp.sec) + nanosecToSec(stamp.nanosec); 125 | } 126 | 127 | } // namespace filter_utilities 128 | } // namespace robot_localization 129 | 130 | #endif // ROBOT_LOCALIZATION__FILTER_UTILITIES_HPP_ 131 | -------------------------------------------------------------------------------- /include/robot_localization/measurement.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * Copyright (c) 2017, Locus Robotics, Inc. 4 | * Copyright (c) 2019, Steve Macenski 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * 3. Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROBOT_LOCALIZATION__MEASUREMENT_HPP_ 36 | #define ROBOT_LOCALIZATION__MEASUREMENT_HPP_ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace robot_localization 49 | { 50 | 51 | /** 52 | * @brief Structure used for storing and comparing measurements 53 | * (for priority queues) 54 | * 55 | * Measurement units are assumed to be in meters and radians. Times are 56 | * real-valued and measured in seconds. 57 | */ 58 | struct Measurement 59 | { 60 | Measurement() 61 | : time_(0), mahalanobis_thresh_(std::numeric_limits::max()), 62 | latest_control_time_(0), topic_name_(""), latest_control_() 63 | { 64 | } 65 | 66 | // The real-valued time, in seconds, since some epoch (presumably the start of 67 | // execution, but any will do) 68 | rclcpp::Time time_; 69 | 70 | // The Mahalanobis distance threshold in number of sigmas 71 | double mahalanobis_thresh_; 72 | 73 | // The time stamp of the most recent control term (needed for lagged data) 74 | rclcpp::Time latest_control_time_; 75 | 76 | // The topic name for this measurement. Needed for capturing previous state 77 | // values for new measurements. 78 | std::string topic_name_; 79 | 80 | // This defines which variables within this measurement actually get passed 81 | // into the filter. std::vector is generally frowned upon, so we use 82 | // ints. 83 | std::vector update_vector_; 84 | 85 | // The most recent control vector (needed for lagged data) 86 | Eigen::VectorXd latest_control_; 87 | 88 | // The measurement and its associated covariance 89 | Eigen::VectorXd measurement_; 90 | Eigen::MatrixXd covariance_; 91 | 92 | // We want earlier times to have greater priority 93 | bool operator()( 94 | const std::shared_ptr & a, 95 | const std::shared_ptr & b) 96 | { 97 | return (*this)(*(a.get()), *(b.get())); 98 | } 99 | 100 | bool operator()(const Measurement & a, const Measurement & b) 101 | { 102 | return a.time_ > b.time_; 103 | } 104 | }; 105 | using MeasurementPtr = std::shared_ptr; 106 | 107 | } // namespace robot_localization 108 | 109 | #endif // ROBOT_LOCALIZATION__MEASUREMENT_HPP_ 110 | -------------------------------------------------------------------------------- /include/robot_localization/ros_filter_types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef ROBOT_LOCALIZATION__ROS_FILTER_TYPES_HPP_ 34 | #define ROBOT_LOCALIZATION__ROS_FILTER_TYPES_HPP_ 35 | 36 | #include "robot_localization/ros_filter.hpp" 37 | #include "robot_localization/ekf.hpp" 38 | #include "robot_localization/ukf.hpp" 39 | 40 | namespace robot_localization 41 | { 42 | typedef RosFilter RosUkf; 43 | typedef RosFilter RosEkf; 44 | } 45 | 46 | #endif // ROBOT_LOCALIZATION__ROS_FILTER_TYPES_HPP_ 47 | -------------------------------------------------------------------------------- /include/robot_localization/ros_filter_utilities.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef ROBOT_LOCALIZATION__ROS_FILTER_UTILITIES_HPP_ 34 | #define ROBOT_LOCALIZATION__ROS_FILTER_UTILITIES_HPP_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #define RF_DEBUG(msg) \ 49 | if (filter_.getDebug()) { \ 50 | debug_stream_ << msg; \ 51 | } 52 | 53 | // Handy methods for debug output 54 | std::ostream & operator<<(std::ostream & os, const tf2::Vector3 & vec); 55 | std::ostream & operator<<(std::ostream & os, const tf2::Quaternion & quat); 56 | std::ostream & operator<<(std::ostream & os, const tf2::Transform & trans); 57 | std::ostream & operator<<(std::ostream & os, const std::vector & vec); 58 | std::ostream & operator<<(std::ostream & os, const std::vector & vec); 59 | 60 | namespace robot_localization 61 | { 62 | namespace ros_filter_utilities 63 | { 64 | 65 | double getYaw(const tf2::Quaternion quat); 66 | 67 | //! @brief Method for safely obtaining transforms. 68 | //! @param[in] buffer - tf buffer object to use for looking up the transform 69 | //! @param[in] targetFrame - The target frame of the desired transform 70 | //! @param[in] sourceFrame - The source frame of the desired transform 71 | //! @param[in] time - The time at which we want the transform 72 | //! @param[in] timeout - How long to block before falling back to last transform 73 | //! @param[out] targetFrameTrans - The resulting transform object 74 | //! @param[in] silent - Whether or not to print transform warnings 75 | //! @return Sets the value of @p targetFrameTrans and returns true if 76 | //! successful, false otherwise. 77 | //! 78 | //! This method attempts to obtain a transform from the @p sourceFrame to the @p 79 | //! targetFrame at the specific @p time. If no transform is available at that 80 | //! time, it attempts to simply obtain the latest transform. If that still 81 | //! fails, then the method checks to see if the transform is going from a given 82 | //! frame_id to itself. If any of these checks succeed, the method sets the 83 | //! value of @p targetFrameTrans and returns true, otherwise it returns false. 84 | //! 85 | bool lookupTransformSafe( 86 | const tf2_ros::Buffer * buffer, 87 | const std::string & target_frame, 88 | const std::string & source_frame, 89 | const rclcpp::Time & time, 90 | const rclcpp::Duration & timeout, 91 | tf2::Transform & target_frame_trans, 92 | const bool silent = false); 93 | 94 | //! @brief Method for safely obtaining transforms. 95 | //! @param[in] buffer - tf buffer object to use for looking up the transform 96 | //! @param[in] targetFrame - The target frame of the desired transform 97 | //! @param[in] sourceFrame - The source frame of the desired transform 98 | //! @param[in] time - The time at which we want the transform 99 | //! @param[out] targetFrameTrans - The resulting transform object 100 | //! @param[in] silent - Whether or not to print transform warnings 101 | //! @return Sets the value of @p targetFrameTrans and returns true if 102 | //! successful, false otherwise. 103 | //! 104 | //! This method attempts to obtain a transform from the @p sourceFrame to the @p 105 | //! targetFrame at the specific @p time. If no transform is available at that 106 | //! time, it attempts to simply obtain the latest transform. If that still 107 | //! fails, then the method checks to see if the transform is going from a given 108 | //! frame_id to itself. If any of these checks succeed, the method sets the 109 | //! value of @p targetFrameTrans and returns true, otherwise it returns false. 110 | //! 111 | bool lookupTransformSafe( 112 | const tf2_ros::Buffer * buffer, 113 | const std::string & targetFrame, 114 | const std::string & sourceFrame, 115 | const rclcpp::Time & time, 116 | tf2::Transform & targetFrameTrans, 117 | const bool silent = false); 118 | 119 | //! @brief Utility method for converting quaternion to RPY 120 | //! @param[in] quat - The quaternion to convert 121 | //! @param[out] roll - The converted roll 122 | //! @param[out] pitch - The converted pitch 123 | //! @param[out] yaw - The converted yaw 124 | //! 125 | void quatToRPY( 126 | const tf2::Quaternion & quat, double & roll, double & pitch, 127 | double & yaw); 128 | 129 | //! @brief Converts our Eigen state vector into a TF transform/pose 130 | //! @param[in] state - The state to convert 131 | //! @param[out] stateTF - The converted state 132 | //! 133 | void stateToTF(const Eigen::VectorXd & state, tf2::Transform & stateTF); 134 | 135 | //! @brief Converts a TF transform/pose into our Eigen state vector 136 | //! @param[in] stateTF - The state to convert 137 | //! @param[out] state - The converted state 138 | //! 139 | void TFtoState(const tf2::Transform & stateTF, Eigen::VectorXd & state); 140 | 141 | } // namespace ros_filter_utilities 142 | } // namespace robot_localization 143 | 144 | #endif // ROBOT_LOCALIZATION__ROS_FILTER_UTILITIES_HPP_ 145 | -------------------------------------------------------------------------------- /include/robot_localization/ros_robot_localization_listener.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, TNO IVS Helmond. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef ROBOT_LOCALIZATION__ROS_ROBOT_LOCALIZATION_LISTENER_HPP_ 34 | #define ROBOT_LOCALIZATION__ROS_ROBOT_LOCALIZATION_LISTENER_HPP_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | #include "robot_localization/robot_localization_estimator.hpp" 48 | 49 | namespace robot_localization 50 | { 51 | 52 | //! @brief RosRobotLocalizationListener class 53 | //! 54 | //! This class wraps the RobotLocalizationEstimator. It listens to topics over 55 | //! which the (filtered) robot state is published (odom and accel) and pushes 56 | //! them into its instance of the RobotLocalizationEstimator. It exposes a 57 | //! getState method to offer the user the estimated state at a requested time. 58 | //! This class offers the option to run this listener without the need to run a 59 | //! separate node. If you do wish to run this functionality in a separate node, 60 | //! consider the robot localization listener node. 61 | //! 62 | class RosRobotLocalizationListener 63 | { 64 | public: 65 | //! @brief Constructor 66 | //! 67 | //! The RosRobotLocalizationListener constructor initializes nodehandles, 68 | //! subscribers, a filter for synchronized listening to the topics it 69 | //! subscribes to, and an instance of the RobotLocalizationEstimator. 70 | //! 71 | //! @param[in] node - rclcpp node shared pointer 72 | //! 73 | explicit RosRobotLocalizationListener(rclcpp::Node::SharedPtr node); 74 | 75 | //! @brief Destructor 76 | //! 77 | //! Empty destructor 78 | //! 79 | ~RosRobotLocalizationListener(); 80 | 81 | //! @brief Get a state from the localization estimator 82 | //! 83 | //! Requests the predicted state and covariance at a given time from the 84 | //! Robot Localization Estimator. 85 | //! 86 | //! @param[in] time - time of the requested state 87 | //! @param[in] frame_id - frame id of which the state is requested. 88 | //! @param[out] state - state at the requested time 89 | //! @param[out] covariance - covariance at the requested time 90 | //! 91 | //! @return false if buffer is empty, true otherwise 92 | //! 93 | bool getState( 94 | const double time, const std::string & frame_id, 95 | Eigen::VectorXd & state, Eigen::MatrixXd & covariance, 96 | std::string world_frame_id = "") const; 97 | 98 | //! @brief Get a state from the localization estimator 99 | //! 100 | //! Overload of getState method for using ros::Time. 101 | //! 102 | //! @param[in] rclcpp_time - ros time of the requested state 103 | //! @param[in] frame_id - frame id of which the state is requested. 104 | //! @param[out] state - state at the requested time 105 | //! @param[out] covariance - covariance at the requested time 106 | //! 107 | //! @return false if buffer is empty, true otherwise 108 | //! 109 | bool getState( 110 | const rclcpp::Time & rclcpp_time, const std::string & frame_id, 111 | Eigen::VectorXd & state, Eigen::MatrixXd & covariance, 112 | const std::string & world_frame_id = "") const; 113 | 114 | //! 115 | //! \brief getBaseFrameId Returns the base frame id of the localization 116 | //! listener 117 | //! \return The base frame id 118 | //! 119 | const std::string & getBaseFrameId() const; 120 | 121 | //! 122 | //! \brief getWorldFrameId Returns the world frame id of the localization 123 | //! listener 124 | //! \return The world frame id 125 | //! 126 | const std::string & getWorldFrameId() const; 127 | 128 | private: 129 | //! @brief Callback for odom and accel 130 | //! 131 | //! Puts the information from the odom and accel messages in a Robot 132 | //! Localization Estimator state and sets the most 133 | //! recent state of the estimator. 134 | //! 135 | //! @param[in] odometry message 136 | //! @param[in] accel message 137 | //! 138 | void odomAndAccelCallback( 139 | const std::shared_ptr & odom, 140 | const std::shared_ptr & 141 | accel); 142 | 143 | //! @brief The core state estimator that facilitates inter- and 144 | //! extrapolation between buffered states. 145 | //! 146 | std::unique_ptr estimator_; 147 | 148 | //! @brief Quality of service definitions 149 | //! 150 | rclcpp::QoS qos1_, qos10_; 151 | 152 | //! @brief Subscriber to the odometry state topic (output of a filter node) 153 | //! 154 | message_filters::Subscriber odom_sub_; 155 | 156 | //! @brief Subscriber to the acceleration state topic (output of a filter 157 | //! node) 158 | //! 159 | message_filters::Subscriber 160 | accel_sub_; 161 | 162 | //! @brief Waits for both an Odometry and an Accel message before calling a 163 | //! single callback function 164 | //! 165 | message_filters::TimeSynchronizer sync_; 167 | 168 | //! @brief rclcpp interface to clock 169 | //! 170 | rclcpp::Clock::SharedPtr node_clock_; 171 | 172 | //! @brief rclcpp interface to logging 173 | //! 174 | rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logger_; 175 | 176 | //! @brief Child frame id received from the Odometry message 177 | //! 178 | std::string base_frame_id_; 179 | 180 | //! @brief Frame id received from the odometry message 181 | //! 182 | std::string world_frame_id_; 183 | 184 | //! @brief Tf buffer for looking up transforms 185 | //! 186 | tf2_ros::Buffer tf_buffer_; 187 | 188 | //! @brief Transform listener to fill the tf_buffer 189 | //! 190 | tf2_ros::TransformListener tf_listener_; 191 | }; 192 | 193 | } // namespace robot_localization 194 | 195 | #endif // ROBOT_LOCALIZATION__ROS_ROBOT_LOCALIZATION_LISTENER_HPP_ 196 | -------------------------------------------------------------------------------- /include/robot_localization/ukf.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef ROBOT_LOCALIZATION__UKF_HPP_ 34 | #define ROBOT_LOCALIZATION__UKF_HPP_ 35 | 36 | #include 37 | #include 38 | 39 | namespace robot_localization 40 | { 41 | 42 | /** 43 | * @brief Unscented Kalman filter class 44 | * 45 | * Implementation of an unscenter Kalman filter (UKF). This class derives from 46 | * FilterBase and overrides the predict() and correct() methods in keeping with 47 | * the discrete time UKF algorithm. The algorithm was derived from the UKF 48 | * Wikipedia article at 49 | * http://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter 50 | * as well as this paper: 51 | * J. J. LaViola, Jr., “A comparison of unscented and extended Kalman filtering 52 | * for estimating quaternion motion,” in Proc. American Control Conf., Denver, 53 | * CO, June 4–6, 2003, pp. 2435–2440 Obtained here: 54 | * http://www.cs.ucf.edu/~jjl/pubs/laviola_acc2003.pdf 55 | */ 56 | class Ukf : public FilterBase 57 | { 58 | public: 59 | /** 60 | * @brief Constructor for the Ukf class 61 | * 62 | * @param[in] args - Generic argument container. It is assumed that args[0] 63 | * constains the alpha parameter, args[1] contains the kappa parameter, and 64 | * args[2] contains the beta parameter. 65 | */ 66 | Ukf(); 67 | 68 | /** 69 | * @brief Destructor for the Ukf class 70 | */ 71 | ~Ukf(); 72 | 73 | void setConstants(double alpha, double kappa, double beta); 74 | 75 | /** 76 | * @brief Carries out the correct step in the predict/update cycle. 77 | * 78 | * @param[in] measurement - The measurement to fuse with our estimate 79 | */ 80 | void correct(const Measurement & measurement) override; 81 | 82 | /** 83 | * @brief Carries out the predict step in the predict/update cycle. 84 | * 85 | * Projects the state and error matrices forward using a model of the 86 | * vehicle's motion. 87 | * 88 | * @param[in] reference_time - The time at which the prediction is being made 89 | * @param[in] delta - The time step over which to predict. 90 | */ 91 | void predict( 92 | const rclcpp::Time & reference_time, 93 | const rclcpp::Duration & delta) override; 94 | 95 | protected: 96 | /** 97 | * @brief The UKF sigma points 98 | * 99 | * Used to sample possible next states during prediction. 100 | */ 101 | std::vector sigma_points_; 102 | 103 | /** 104 | * @brief This matrix is used to generate the sigmaPoints_ 105 | */ 106 | Eigen::MatrixXd weighted_covar_sqrt_; 107 | 108 | /** 109 | * @brief The weights associated with each sigma point when generating a new 110 | * state 111 | */ 112 | std::vector state_weights_; 113 | 114 | /** 115 | * @brief The weights associated with each sigma point when calculating a 116 | * predicted estimateErrorCovariance_ 117 | */ 118 | std::vector covar_weights_; 119 | 120 | /** 121 | * @brief Used in weight generation for the sigma points 122 | */ 123 | double lambda_; 124 | 125 | /** 126 | * @brief Used to determine if we need to re-compute the sigma points when 127 | * carrying out multiple corrections 128 | */ 129 | bool uncorrected_; 130 | }; 131 | 132 | } // namespace robot_localization 133 | 134 | #endif // ROBOT_LOCALIZATION__UKF_HPP_ 135 | -------------------------------------------------------------------------------- /launch/dual_ekf_navsat_example.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # Licensed under the Apache License, Version 2.0 (the "License"); 3 | # you may not use this file except in compliance with the License. 4 | # You may obtain a copy of the License at 5 | # 6 | # http://www.apache.org/licenses/LICENSE-2.0 7 | # 8 | # Unless required by applicable law or agreed to in writing, software 9 | # distributed under the License is distributed on an "AS IS" BASIS, 10 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | # See the License for the specific language governing permissions and 12 | # limitations under the License. 13 | 14 | from launch import LaunchDescription 15 | import launch_ros.actions 16 | import os 17 | import yaml 18 | from launch.substitutions import EnvironmentVariable 19 | import pathlib 20 | import launch.actions 21 | from launch.actions import DeclareLaunchArgument 22 | from ament_index_python.packages import get_package_share_directory 23 | 24 | def generate_launch_description(): 25 | robot_localization_dir = get_package_share_directory('robot_localization') 26 | parameters_file_dir = os.path.join(robot_localization_dir, 'params') 27 | parameters_file_path = os.path.join(parameters_file_dir, 'dual_ekf_navsat_example.yaml') 28 | os.environ['FILE_PATH'] = str(parameters_file_dir) 29 | return LaunchDescription([ 30 | launch.actions.DeclareLaunchArgument( 31 | 'output_final_position', 32 | default_value='false'), 33 | launch.actions.DeclareLaunchArgument( 34 | 'output_location', 35 | default_value='~/dual_ekf_navsat_example_debug.txt'), 36 | 37 | launch_ros.actions.Node( 38 | package='robot_localization', 39 | executable='ekf_node', 40 | name='ekf_filter_node_odom', 41 | output='screen', 42 | parameters=[parameters_file_path], 43 | remappings=[('odometry/filtered', 'odometry/local')] 44 | ), 45 | launch_ros.actions.Node( 46 | package='robot_localization', 47 | executable='ekf_node', 48 | name='ekf_filter_node_map', 49 | output='screen', 50 | parameters=[parameters_file_path], 51 | remappings=[('odometry/filtered', 'odometry/global')] 52 | ), 53 | launch_ros.actions.Node( 54 | package='robot_localization', 55 | executable='navsat_transform_node', 56 | name='navsat_transform', 57 | output='screen', 58 | parameters=[parameters_file_path], 59 | remappings=[('imu/data', 'imu/data'), 60 | ('gps/fix', 'gps/fix'), 61 | ('gps/filtered', 'gps/filtered'), 62 | ('odometry/gps', 'odometry/gps'), 63 | ('odometry/filtered', 'odometry/global')] 64 | 65 | ) 66 | ]) 67 | -------------------------------------------------------------------------------- /launch/ekf.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # Copyright 2019 Samsung Research America 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from launch import LaunchDescription 17 | from ament_index_python.packages import get_package_share_directory 18 | import launch_ros.actions 19 | import os 20 | import yaml 21 | from launch.substitutions import EnvironmentVariable 22 | import pathlib 23 | import launch.actions 24 | from launch.actions import DeclareLaunchArgument 25 | 26 | def generate_launch_description(): 27 | return LaunchDescription([ 28 | launch_ros.actions.Node( 29 | package='robot_localization', 30 | executable='ekf_node', 31 | name='ekf_filter_node', 32 | output='screen', 33 | parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'ekf.yaml')], 34 | ), 35 | ]) 36 | -------------------------------------------------------------------------------- /launch/navsat_transform.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Samsung Research America 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from ament_index_python.packages import get_package_share_directory 17 | import launch_ros.actions 18 | import os 19 | import yaml 20 | from launch.substitutions import EnvironmentVariable 21 | import pathlib 22 | import launch.actions 23 | from launch.actions import DeclareLaunchArgument 24 | 25 | def generate_launch_description(): 26 | return LaunchDescription([ 27 | launch_ros.actions.Node( 28 | package='robot_localization', 29 | executable='navsat_transform_node', 30 | name='navsat_transform_node', 31 | output='screen', 32 | parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'navsat_transform.yaml')], 33 | ), 34 | ]) 35 | -------------------------------------------------------------------------------- /launch/ukf.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # Copyright 2019 Samsung Research America 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from launch import LaunchDescription 17 | from ament_index_python.packages import get_package_share_directory 18 | import launch_ros.actions 19 | import os 20 | import yaml 21 | from launch.substitutions import EnvironmentVariable 22 | import pathlib 23 | import launch.actions 24 | from launch.actions import DeclareLaunchArgument 25 | 26 | def generate_launch_description(): 27 | return LaunchDescription([ 28 | launch_ros.actions.Node( 29 | package='robot_localization', 30 | executable='ukf_node', 31 | name='ukf_filter_node', 32 | output='screen', 33 | parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'ukf.yaml')], 34 | ), 35 | ]) 36 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_localization 5 | 3.2.3 6 | Provides nonlinear state estimation through sensor fusion of an abritrary number of sensors. 7 | 8 | Tom Moore 9 | Tom Moore 10 | Steve Macenski 11 | 12 | Apache License 2.0 13 | 14 | http://ros.org/wiki/robot_localization 15 | 16 | ament_cmake 17 | builtin_interfaces 18 | rosidl_default_generators 19 | 20 | libboost-dev 21 | libboost-dev 22 | 23 | eigen 24 | geographic_msgs 25 | geometry_msgs 26 | diagnostic_msgs 27 | diagnostic_updater 28 | geographiclib 29 | message_filters 30 | nav_msgs 31 | rclcpp 32 | rmw_implementation 33 | sensor_msgs 34 | std_msgs 35 | std_srvs 36 | tf2 37 | tf2_eigen 38 | tf2_geometry_msgs 39 | tf2_ros 40 | yaml_cpp_vendor 41 | 42 | ament_cmake_gtest 43 | builtin_interfaces 44 | rosidl_default_runtime 45 | 46 | rclcpp 47 | rmw_implementation 48 | 49 | ament_lint_auto 50 | ament_lint_common 51 | launch_ros 52 | launch_testing_ament_cmake 53 | 54 | rosidl_interface_packages 55 | 56 | 57 | ament_cmake 58 | 59 | 60 | -------------------------------------------------------------------------------- /params/navsat_transform.yaml: -------------------------------------------------------------------------------- 1 | navsat_transform_node: 2 | ros__parameters: 3 | # Frequency of the main run loop 4 | frequency: 30.0 5 | 6 | # Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially 7 | # important if you have use_odometry_yaw set to true. Defaults to 0. 8 | delay: 3.0 9 | 10 | # PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame. 11 | # Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using 12 | # it. 13 | 14 | # If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it, 15 | # see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory. 16 | magnetic_declination_radians: 0.0 17 | 18 | # Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it 19 | # doesn't, enter the offset here. Defaults to 0. 20 | yaw_offset: 0.0 21 | 22 | # If this is true, the altitude is set to 0 in the output odometry message. Defaults to false. 23 | zero_altitude: false 24 | 25 | # If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false. 26 | broadcast_utm_transform: false 27 | 28 | # If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform. 29 | # Note that broadcast_utm_transform still has to be enabled. Defaults to false. 30 | broadcast_utm_transform_as_parent_frame: false 31 | 32 | # If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as 33 | # /gps/filtered. Defaults to false. 34 | publish_filtered_gps: false 35 | 36 | # If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the 37 | # /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this! 38 | # The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw 39 | # if your yaw data is based purely on integrated velocities. Defaults to false. 40 | use_odometry_yaw: false 41 | 42 | # If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists, 43 | # navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message. 44 | wait_for_datum: false 45 | 46 | # Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the 47 | # origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees, 48 | # and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east. 49 | datum: [55.944904, -3.186693, 0.0] 50 | 51 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: doc 3 | - builder: doxygen 4 | output_dir: api 5 | file_patterns: '*.cpp *.h *.dox *.md' 6 | exclude_patterns: '*/test/*' 7 | -------------------------------------------------------------------------------- /src/ekf_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Locus Robotics 3 | * Copyright (c) 2019, Steve Macenski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | int main(int argc, char ** argv) 43 | { 44 | rclcpp::init(argc, argv); 45 | rclcpp::NodeOptions options; 46 | options.arguments({"ekf_filter_node"}); 47 | std::shared_ptr filter = 48 | std::make_shared(options); 49 | filter->initialize(); 50 | rclcpp::spin(filter->get_node_base_interface()); 51 | rclcpp::shutdown(); 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /src/filter_utilities.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | std::ostream & operator<<(std::ostream & os, const Eigen::MatrixXd & mat) 41 | { 42 | os << "["; 43 | 44 | int row_count = static_cast(mat.rows()); 45 | 46 | for (int row = 0; row < row_count; ++row) { 47 | if (row > 0) { 48 | os << " "; 49 | } 50 | 51 | for (int col = 0; col < mat.cols(); ++col) { 52 | os << std::setiosflags(std::ios::left) << std::setw(12) << 53 | std::setprecision(5) << mat(row, col); 54 | } 55 | 56 | if (row < row_count - 1) { 57 | os << "\n"; 58 | } 59 | } 60 | 61 | os << "]\n"; 62 | 63 | return os; 64 | } 65 | 66 | std::ostream & operator<<(std::ostream & os, const Eigen::VectorXd & vec) 67 | { 68 | os << "["; 69 | for (int dim = 0; dim < vec.rows(); ++dim) { 70 | os << std::setiosflags(std::ios::left) << std::setw(12) << 71 | std::setprecision(5) << vec(dim); 72 | } 73 | os << "]\n"; 74 | 75 | return os; 76 | } 77 | 78 | std::ostream & operator<<(std::ostream & os, const std::vector & vec) 79 | { 80 | os << "["; 81 | for (size_t dim = 0; dim < vec.size(); ++dim) { 82 | os << std::setiosflags(std::ios::left) << std::setw(12) << 83 | std::setprecision(5) << vec[dim]; 84 | } 85 | os << "]\n"; 86 | 87 | return os; 88 | } 89 | 90 | std::ostream & operator<<(std::ostream & os, const std::vector & vec) 91 | { 92 | os << "["; 93 | for (size_t dim = 0; dim < vec.size(); ++dim) { 94 | os << std::setiosflags(std::ios::left) << std::setw(3) << 95 | (vec[dim] ? "t" : "f"); 96 | } 97 | os << "]\n"; 98 | 99 | return os; 100 | } 101 | -------------------------------------------------------------------------------- /src/navsat_transform_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | int main(int argc, char ** argv) 38 | { 39 | rclcpp::init(argc, argv); 40 | 41 | const rclcpp::NodeOptions options; 42 | auto navsat_transform_node = std::make_shared(options); 43 | 44 | rclcpp::spin(navsat_transform_node->get_node_base_interface()); 45 | 46 | rclcpp::shutdown(); 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /src/robot_localization_listener_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, TNO IVS Helmond. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include "robot_localization/ros_robot_localization_listener.hpp" 42 | #include "robot_localization/srv/get_state.hpp" 43 | 44 | namespace robot_localization 45 | { 46 | 47 | class RobotLocalizationListenerNode : public rclcpp::Node 48 | { 49 | public: 50 | RobotLocalizationListenerNode() 51 | : rclcpp::Node("robot_localization_listener_node") 52 | { 53 | service_ = this->create_service( 54 | "get_state", 55 | std::bind( 56 | &RobotLocalizationListenerNode::getStateCallback, this, 57 | std::placeholders::_1, std::placeholders::_2)); 58 | } 59 | 60 | std::string getService() 61 | { 62 | return std::string(service_->get_service_name()); 63 | } 64 | 65 | void setRosRobotLocalizationListener( 66 | std::shared_ptr rll) 67 | { 68 | rll_ = rll; 69 | } 70 | 71 | private: 72 | std::shared_ptr rll_; 73 | rclcpp::Service::SharedPtr service_; 74 | 75 | bool getStateCallback( 76 | const std::shared_ptr req, 77 | const std::shared_ptr res) 78 | { 79 | Eigen::VectorXd state(STATE_SIZE); 80 | Eigen::MatrixXd covariance(STATE_SIZE, STATE_SIZE); 81 | 82 | if (!rll_->getState(req->time_stamp, req->frame_id, state, covariance)) { 83 | RCLCPP_ERROR( 84 | this->get_logger(), 85 | "Robot Localization Listener Node: Listener instance returned false at " 86 | "getState call."); 87 | return false; 88 | } 89 | 90 | for (size_t i = 0; i < STATE_SIZE; i++) { 91 | res->state[i] = (*(state.data() + i)); 92 | } 93 | 94 | for (size_t i = 0; i < STATE_SIZE * STATE_SIZE; i++) { 95 | res->covariance[i] = (*(covariance.data() + i)); 96 | } 97 | 98 | RCLCPP_DEBUG( 99 | this->get_logger(), 100 | "Robot Localization Listener Node: Listener responded with state and " 101 | "covariance at the requested time."); 102 | return true; 103 | } 104 | }; 105 | 106 | } // namespace robot_localization 107 | 108 | int main(int argc, char ** argv) 109 | { 110 | rclcpp::init(argc, argv); 111 | auto rlln = std::make_shared< 112 | robot_localization::RobotLocalizationListenerNode>(); 113 | 114 | auto rll = std::make_shared< 115 | robot_localization::RosRobotLocalizationListener>(rlln); 116 | rlln->setRosRobotLocalizationListener(rll); 117 | 118 | RCLCPP_INFO( 119 | rlln->get_logger(), 120 | "Robot Localization Listener Node: Ready to handle GetState requests at %s", 121 | rlln->getService().c_str()); 122 | 123 | rclcpp::spin(rlln); 124 | 125 | return 0; 126 | } 127 | -------------------------------------------------------------------------------- /src/ukf_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Locus Robotics 3 | * Copyright (c) 2019, Steve Macenski 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * 3. Neither the name of the copyright holder nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | */ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | int main(int argc, char ** argv) 43 | { 44 | rclcpp::init(argc, argv); 45 | rclcpp::NodeOptions options; 46 | options.arguments({"ukf_filter_node"}); 47 | std::shared_ptr filter = 48 | std::make_shared(options); 49 | double alpha = filter->declare_parameter("alpha", 0.001); 50 | double kappa = filter->declare_parameter("kappa", 0.0); 51 | double beta = filter->declare_parameter("beta", 2.0); 52 | filter->getFilter().setConstants(alpha, kappa, beta); 53 | filter->initialize(); 54 | rclcpp::spin(filter->get_node_base_interface()); 55 | rclcpp::shutdown(); 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /srv/FromLL.srv: -------------------------------------------------------------------------------- 1 | geographic_msgs/GeoPoint ll_point 2 | --- 3 | geometry_msgs/Point map_point 4 | -------------------------------------------------------------------------------- /srv/GetState.srv: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time time_stamp 2 | string frame_id 3 | --- 4 | # State vector: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az 5 | float64[15] state 6 | 7 | # Covariance matrix in row-major order 8 | float64[225] covariance 9 | -------------------------------------------------------------------------------- /srv/SetDatum.srv: -------------------------------------------------------------------------------- 1 | geographic_msgs/GeoPose geo_pose 2 | --- 3 | -------------------------------------------------------------------------------- /srv/SetPose.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseWithCovarianceStamped pose 2 | --- 3 | -------------------------------------------------------------------------------- /srv/ToLL.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point map_point 2 | --- 3 | geographic_msgs/GeoPoint ll_point 4 | -------------------------------------------------------------------------------- /srv/ToggleFilterProcessing.srv: -------------------------------------------------------------------------------- 1 | bool on 2 | --- 3 | bool status 4 | -------------------------------------------------------------------------------- /test/test1.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/automaticaddison/robot_localization/f77f5a6e5f986a6928c9eea40b5d9c6e600b42a9/test/test1.bag -------------------------------------------------------------------------------- /test/test2.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/automaticaddison/robot_localization/f77f5a6e5f986a6928c9eea40b5d9c6e600b42a9/test/test2.bag -------------------------------------------------------------------------------- /test/test3.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/automaticaddison/robot_localization/f77f5a6e5f986a6928c9eea40b5d9c6e600b42a9/test/test3.bag -------------------------------------------------------------------------------- /test/test_ekf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "robot_localization/ekf.hpp" 41 | #include "robot_localization/ros_filter.hpp" 42 | #include "robot_localization/ros_filter_types.hpp" 43 | 44 | using robot_localization::Ekf; 45 | using robot_localization::RosEkf; 46 | using robot_localization::STATE_SIZE; 47 | 48 | TEST(EkfTest, Measurements) { 49 | // node handle is created as per ros2 50 | rclcpp::NodeOptions options; 51 | options.arguments({"ekf_filter_node"}); 52 | std::shared_ptr filter = 53 | std::make_shared(options); 54 | filter->initialize(); 55 | 56 | // create the instance of the class and pass parameters 57 | Eigen::MatrixXd initialCovar(15, 15); 58 | 59 | initialCovar.setIdentity(); 60 | initialCovar *= 0.5; 61 | 62 | filter->getFilter().setEstimateErrorCovariance(initialCovar); 63 | 64 | Eigen::VectorXd measurement(STATE_SIZE); 65 | measurement.setIdentity(); 66 | 67 | for (size_t i = 0; i < STATE_SIZE; ++i) { 68 | measurement[i] = i * 0.01 * STATE_SIZE; 69 | } 70 | Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); 71 | measurementCovariance.setIdentity(); 72 | for (size_t i = 0; i < STATE_SIZE; ++i) { 73 | measurementCovariance(i, i) = 1e-9; 74 | } 75 | std::vector updateVector(STATE_SIZE, true); 76 | 77 | // Ensure that measurements are being placed in the queue correctly 78 | rclcpp::Time time1(1000); 79 | filter->robot_localization::RosEkf::enqueueMeasurement( 80 | "odom0", measurement, measurementCovariance, updateVector, 81 | std::numeric_limits::max(), time1); 82 | 83 | filter->robot_localization::RosEkf::integrateMeasurements(rclcpp::Time(1001)); 84 | 85 | EXPECT_EQ(filter->getFilter().getState(), measurement); 86 | EXPECT_EQ( 87 | filter->getFilter().getEstimateErrorCovariance(), 88 | measurementCovariance); 89 | 90 | filter->getFilter().setEstimateErrorCovariance(initialCovar); 91 | 92 | // Now fuse another measurement and check the output. 93 | // We know what the filter's state should be when 94 | // this is complete, so we'll check the difference and 95 | // make sure it's suitably small. 96 | Eigen::VectorXd measurement2 = measurement; 97 | 98 | measurement2 *= 2.0; 99 | 100 | for (size_t i = 0; i < STATE_SIZE; ++i) { 101 | measurementCovariance(i, i) = 1e-9; 102 | } 103 | 104 | rclcpp::Time time2(1002); 105 | 106 | filter->robot_localization::RosEkf::enqueueMeasurement( 107 | "odom0", measurement2, measurementCovariance, updateVector, 108 | std::numeric_limits::max(), time2); 109 | 110 | filter->robot_localization::RosEkf::integrateMeasurements(rclcpp::Time(1003)); 111 | 112 | measurement = measurement2.eval() - filter->getFilter().getState(); 113 | for (size_t i = 0; i < STATE_SIZE; ++i) { 114 | EXPECT_LT(::fabs(measurement[i]), 0.001); 115 | } 116 | } 117 | 118 | int main(int argc, char ** argv) 119 | { 120 | rclcpp::init(argc, argv); 121 | ::testing::InitGoogleTest(&argc, argv); 122 | int ret = RUN_ALL_TESTS(); 123 | rclcpp::shutdown(); 124 | 125 | return ret; 126 | } 127 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag1.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | import launch_ros.actions 17 | import os 18 | import yaml 19 | from launch.substitutions import EnvironmentVariable 20 | import pathlib 21 | import launch.actions 22 | from launch.actions import DeclareLaunchArgument 23 | 24 | 25 | def generate_launch_description(): 26 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 27 | parameters_file_path = parameters_file_dir /'test_ekf_localization_node_bag1.yaml' 28 | os.environ['FILE_PATH'] = str(parameters_file_dir) 29 | return LaunchDescription([ 30 | launch.actions.DeclareLaunchArgument( 31 | 'output_final_position', 32 | default_value='false'), 33 | launch.actions.DeclareLaunchArgument( 34 | 'output_location', 35 | default_value='ekf1.txt'), 36 | 37 | #launch_ros.actions.Node( 38 | # package='tf2_ros', node_executable='static_transform_publisher',node_name='bl_imu', output='screen', 39 | # arguments=['0', '-0.3', '0.52', '-1.570796327', '0', '1.570796327', 'base_link', 'imu_link'] 40 | #), 41 | 42 | launch_ros.actions.Node( 43 | package='robot_localization', executable='ekf_node', name='test_ekf_localization_node_bag1_ekf', 44 | output='screen', 45 | parameters=[ 46 | parameters_file_path, 47 | str(parameters_file_path), 48 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_bag1.yaml'], 49 | ], 50 | ), 51 | 52 | launch_ros.actions.Node( 53 | package='robot_localization', executable='test_ekf_localization_node_bag1', name='test_ekf_localization_node_bag1_pose', 54 | output='screen', 55 | arguments=['--time-limit','10'], 56 | parameters=[ 57 | parameters_file_path, 58 | str(parameters_file_path), 59 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_bag1.yaml'], 60 | ], 61 | ), 62 | ]) 63 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #************************ test_ekf_localization_node_bag1 *************************** 4 | 5 | # In this automatic Script, multiple terminals will be opened, user need to check the terminal on which bag is playing. Once bag is stoped, check the terminal on which launch.py is running, user will see, test cases result (Pass Or Fail) after pressing ctrl+c. 6 | 7 | $PWD = `pwd` 8 | echo "Current Working Directory = $PWD" 9 | ROS1_DISTRO=melodic 10 | ROS2_DISTRO=crystal 11 | echo "ROS1_DISTRO = $ROS1_DISTRO" 12 | echo "ROS2_DISTRO = $ROS2_DISTRO" 13 | 14 | #Command to run roscore 15 | cmd1="source /opt/ros/$ROS1_DISTRO/setup.sh; roscore; exec /bin/bash" 16 | 17 | #Command to run ros1_bridge 18 | cmd2="source ~/ros2_ws/install/setup.bash; source /opt/ros/$ROS1_DISTRO/setup.bash; ros2 run ros1_bridge dynamic_bridge --bridge-all-topics; exec /bin/bash" 19 | 20 | #Command to play .bag from ROS1 21 | cmd3="source /opt/ros/$ROS1_DISTRO/setup.bash; rosparam set use_sim_time true; rosbag play $PWD/src/robot_localization/test/test1.bag --clock -d 5; exec /bin/bash" 22 | 23 | #Command to launch TestCase launch.py 24 | cmd4="source /opt/ros/$ROS2_DISTRO/setup.bash; source $PWD/install/setup.bash; ros2 launch robot_localization test_ekf_localization_node_bag1.launch.py; exec /bin/bash" 25 | 26 | #Command to run static_transform_publisher 27 | cmd5="source /opt/ros/$ROS2_DISTRO/setup.bash; ros2 run tf2_ros static_transform_publisher 0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link; exec /bin/bash" 28 | 29 | gnome-terminal --tab -t "roscore" -- /bin/bash -c "$cmd1" 30 | sleep 1 31 | gnome-terminal --tab -t "ros1_bridge" -- /bin/bash -c "$cmd2" 32 | gnome-terminal --tab -t "bag" -- /bin/bash -c "$cmd3" 33 | gnome-terminal --tab -t "TestCase_launch" -- /bin/bash -c "$cmd4" 34 | gnome-terminal --tab -t "static_transform_publisher" -- /bin/bash -c "$cmd5" 35 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag1.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ekf_localization_node_bag1_ekf: 3 | ros__parameters: 4 | use_sim_time: true 5 | clear_params: true 6 | filter_type: "ekf" 7 | frequency: 30.0 8 | sensor_timeout: 0.1 9 | two_d_mode: false 10 | map_frame: map 11 | odom_frame: odom 12 | base_link_frame: base_link 13 | world_frame: odom 14 | transform_time_offset: 0.0 15 | transform_timeout: 0.0 16 | odom0: /husky_velocity_controller/odom 17 | imu0: /imu/data 18 | odom0_config: [false, false, false, 19 | false, false, false, 20 | true, true, true, 21 | false, false, false, 22 | false, false, false] 23 | imu0_config: [false, false, false, 24 | true, true, true, 25 | false, false, false, 26 | true, true, false, 27 | true, true, true] 28 | odom0_differential: false 29 | imu0_differential: false 30 | odom0_relative: false 31 | imu0_relative: false 32 | imu0_remove_gravitational_acceleration: true 33 | print_diagnostics: true 34 | odom0_queue_size: 10 35 | imu0_queue_size: 10 36 | debug: false 37 | debug_out_file: debug_ekf_localization.txt 38 | process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 39 | 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 44 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 45 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 46 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 48 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 49 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 50 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 51 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 52 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] 53 | 54 | initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55 | 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 56 | 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 57 | 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 58 | 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 59 | 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 60 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 61 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 62 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 63 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 64 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 65 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 66 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 67 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 68 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] 69 | 70 | 71 | # 72 | test_ekf_localization_node_bag1_pose: 73 | ros__parameters: 74 | clear_params: true 75 | final_x: -40.0454 76 | final_y: -76.9988 77 | final_z: -2.6974 78 | tolerance: 1.0452 79 | output_final_position: true 80 | output_location: ekf1.txt 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag2.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | import launch_ros.actions 17 | import os 18 | import yaml 19 | from launch.substitutions import EnvironmentVariable 20 | import pathlib 21 | import launch.actions 22 | from launch.actions import DeclareLaunchArgument 23 | 24 | 25 | 26 | def generate_launch_description(): 27 | 28 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 29 | parameters_file_path = parameters_file_dir / 'test_ekf_localization_node_bag2.yaml' 30 | os.environ['FILE_PATH'] = str(parameters_file_dir) 31 | 32 | ekf_node = launch_ros.actions.Node( 33 | package='robot_localization', executable='ekf_node', name='test_ekf_localization_node_bag2_ekf', 34 | output='screen', 35 | parameters=[ 36 | parameters_file_path, 37 | str(parameters_file_path), 38 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_bag2.yaml'],],) 39 | 40 | test_ekf_localization_node_bag2 = launch_ros.actions.Node( 41 | package='robot_localization', executable='test_ekf_localization_node_bag2', name='test_ekf_localization_node_bag2_pose', 42 | output='screen', 43 | parameters=[ 44 | parameters_file_path, 45 | str(parameters_file_path), 46 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_bag2.yaml'],],) 47 | 48 | return LaunchDescription([ 49 | launch.actions.DeclareLaunchArgument( 50 | 'output_final_position', 51 | default_value='False'), 52 | launch.actions.DeclareLaunchArgument( 53 | 'output_location', 54 | default_value='ekf2.txt'), 55 | ekf_node, 56 | test_ekf_localization_node_bag2, 57 | launch.actions.RegisterEventHandler( 58 | event_handler=launch.event_handlers.OnProcessExit( 59 | target_action=test_ekf_localization_node_bag2, 60 | on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], 61 | )), 62 | ]) 63 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag2.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #************************ test_ekf_localization_node_bag2 *************************** 4 | 5 | # In this automatic Script, multiple terminals will be opened, user need to check the terminal on which bag is playing. Once bag is stoped, check the terminal on which launch.py is running, user will see, test cases result (Pass Or Fail) after pressing ctrl+c. 6 | 7 | $PWD = `pwd` 8 | echo "Current Working Directory = $PWD" 9 | ROS1_DISTRO=melodic 10 | ROS2_DISTRO=crystal 11 | echo "ROS1_DISTRO = $ROS1_DISTRO" 12 | echo "ROS2_DISTRO = $ROS2_DISTRO" 13 | 14 | #Command to run roscore 15 | cmd1="source /opt/ros/$ROS1_DISTRO/setup.sh; roscore; exec /bin/bash" 16 | 17 | #Command to run ros1_bridge 18 | cmd2="source ~/ros2_ws/install/setup.bash; source /opt/ros/$ROS1_DISTRO/setup.bash; ros2 run ros1_bridge dynamic_bridge --bridge-all-topics; exec /bin/bash" 19 | 20 | #Command to play .bag from ROS1 21 | cmd3="source /opt/ros/$ROS1_DISTRO/setup.bash; rosparam set use_sim_time true; rosbag play $PWD/src/robot_localization/test/test2.bag --clock -d 5; exec /bin/bash" 22 | 23 | #Command to launch TestCase launch.py 24 | cmd4="source /opt/ros/$ROS2_DISTRO/setup.bash; source $PWD/install/setup.bash; ros2 launch robot_localization test_ekf_localization_node_bag2.launch.py; exec /bin/bash" 25 | 26 | gnome-terminal --tab -t "roscore" -- /bin/bash -c "$cmd1" 27 | sleep 1 28 | gnome-terminal --tab -t "bag" -- /bin/bash -c "$cmd3" 29 | gnome-terminal --tab -t "TestCase_launch" -- /bin/bash -c "$cmd4" 30 | gnome-terminal --tab -t "ros1_bridge" -- /bin/bash -c "$cmd2" 31 | 32 | 33 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag2.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ekf_localization_node_bag2_ekf: 3 | ros__parameters: 4 | use_sim_time: true 5 | clear_params: true 6 | filter_type: "ekf" 7 | frequency: 50.0 8 | sensor_timeout: 0.1 9 | odom0: /jackal_velocity_controller/odom 10 | imu0: /imu/data 11 | map_frame: map 12 | odom_frame: odom 13 | base_link_frame: base_link 14 | world_frame: odom 15 | odom0_config: [false, false, false, 16 | false, false, false, 17 | true, true, true, 18 | false, false, true, 19 | false, false, false] 20 | odom0_relative: false 21 | imu0_config: [false, false, false, 22 | true, true, true, 23 | false, false, false, 24 | true, true, true, 25 | true, true, true] 26 | odom0_queue_size: 10 27 | imu0_queue_size: 10 28 | imu0_remove_gravitational_acceleration: true 29 | process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 30 | 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 31 | 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 32 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 33 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 34 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 37 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 38 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 39 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] 44 | 45 | initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 46 | 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47 | 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 48 | 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 49 | 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50 | 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 51 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 52 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 54 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 55 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 56 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 57 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 58 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 59 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] 60 | 61 | # 62 | test_ekf_localization_node_bag2_pose: 63 | ros__parameters: 64 | clear_params: true 65 | final_x: 1.0206 66 | final_y: 3.4292 67 | final_z: 0.7419 68 | tolerance: 0.1383 69 | output_final_position: true 70 | output_location: ekf2.txt 71 | 72 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag3.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | import launch_ros.actions 17 | import os 18 | import yaml 19 | from launch.substitutions import EnvironmentVariable 20 | import pathlib 21 | import launch.actions 22 | from launch.actions import DeclareLaunchArgument 23 | 24 | def generate_launch_description(): 25 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 26 | parameters_file_path = parameters_file_dir /'test_ekf_localization_node_bag2.yaml' 27 | os.environ['FILE_PATH'] = str(parameters_file_dir) 28 | return LaunchDescription([ 29 | launch.actions.DeclareLaunchArgument( 30 | 'output_final_position', 31 | default_value='false'), 32 | launch.actions.DeclareLaunchArgument( 33 | 'output_location', 34 | default_value='ekf3.txt'), 35 | 36 | #*****test_ekf_localization_node_bag3.test***** 37 | launch_ros.actions.Node( 38 | package='robot_localization', executable='ekf_node', name='test_ekf_localization_node_bag3_ekf', 39 | output='screen', 40 | parameters=[ 41 | parameters_file_path, 42 | str(parameters_file_path), 43 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_bag3.yaml'], 44 | ], 45 | ), 46 | launch_ros.actions.Node( 47 | package='robot_localization', executable='test_ekf_localization_node_bag3', name='test_ekf_localization_node_bag3_pose', 48 | output='screen', 49 | parameters=[ 50 | parameters_file_path, 51 | str(parameters_file_path), 52 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_bag3.yaml'], 53 | ], 54 | ), 55 | ]) 56 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag3.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #************************ test_ekf_localization_node_bag3 *************************** 4 | 5 | # In this automatic Script, multiple terminals will be opened, user need to check the terminal on which bag is playing. Once bag is stoped, check the terminal on which launch.py is running, user will see, test cases result (Pass Or Fail) after pressing ctrl+c. 6 | 7 | $PWD = `pwd` 8 | echo "Current Working Directory = $PWD" 9 | ROS1_DISTRO=melodic 10 | ROS2_DISTRO=crystal 11 | echo "ROS1_DISTRO = $ROS1_DISTRO" 12 | echo "ROS2_DISTRO = $ROS2_DISTRO" 13 | 14 | #Command to run roscore 15 | cmd1="source /opt/ros/$ROS1_DISTRO/setup.sh; roscore; exec /bin/bash" 16 | 17 | #Command to run ros1_bridge 18 | cmd2="source ~/ros2_ws/install/setup.bash; source /opt/ros/$ROS1_DISTRO/setup.bash; ros2 run ros1_bridge dynamic_bridge --bridge-all-topics; exec /bin/bash" 19 | 20 | #Command to play .bag from ROS1 21 | cmd3="sleep 2; source /opt/ros/$ROS1_DISTRO/setup.bash; rosparam set /use_sim_time true; rosbag play $PWD/src/robot_localization/test/test3.bag --clock -d 5; exec /bin/bash" 22 | 23 | #Command to launch TestCase launch.py 24 | cmd4="sleep 2; source /opt/ros/$ROS2_DISTRO/setup.bash; source $PWD/install/setup.bash; ros2 launch robot_localization test_ekf_localization_node_bag3.launch.py; exec /bin/bash" 25 | 26 | gnome-terminal --tab -t "roscore" -- /bin/bash -c "$cmd1" 27 | sleep 1 28 | gnome-terminal --tab -t "ros1_bridge" -- /bin/bash -c "$cmd2" 29 | gnome-terminal --tab -t "bag" -- /bin/bash -c "$cmd3" 30 | gnome-terminal --tab -t "TestCase_launch" -- /bin/bash -c "$cmd4" 31 | 32 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_bag3.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ekf_localization_node_bag3_ekf: 3 | ros__parameters: 4 | use_sim_time: true 5 | clear_params: true 6 | filter_type: "ekf" 7 | frequency: 30.0 8 | sensor_timeout: 0.1 9 | odom0: /ardrone/odometry/raw 10 | imu0: /ardrone/imu 11 | map_frame: map 12 | odom_frame: odom 13 | base_link_frame: base_link 14 | world_frame: odom 15 | odom0_config: [false, false, true, 16 | false, false, false, 17 | true, true, false, 18 | false, false, true, 19 | false, false, false] 20 | odom0_relative: false 21 | imu0_config: [false, false, false, 22 | true, true, true, 23 | false, false, false, 24 | true, true, true, 25 | false, false, false] 26 | odom0_queue_size: 10 27 | imu0_queue_size: 10 28 | imu0_remove_gravitational_acceleration: true 29 | process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 30 | 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 31 | 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 32 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 33 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 34 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 37 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 38 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 39 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] 44 | 45 | # 46 | test_ekf_localization_node_bag3_pose: 47 | ros__parameters: 48 | clear_params: true 49 | final_x: -0.4859 50 | final_y: -0.2412 51 | final_z: 2.9883 52 | tolerance: 0.1290 53 | output_final_position: true 54 | output_location: ekf3.txt 55 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_interfaces.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2018 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """Launch file for test_ekf_localization_node_interfaces.""" 17 | 18 | import launch 19 | from launch import LaunchDescription 20 | import launch_ros.actions 21 | import os 22 | import yaml 23 | import sys 24 | from launch.substitutions import EnvironmentVariable 25 | import pathlib 26 | import launch.actions 27 | from launch.actions import DeclareLaunchArgument 28 | from launch_testing.legacy import LaunchTestService 29 | from launch.actions import ExecuteProcess 30 | from launch import LaunchService 31 | from ament_index_python.packages import get_package_prefix 32 | 33 | def generate_launch_description(): 34 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 35 | parameters_file_path = parameters_file_dir / 'test_ekf_localization_node_interfaces.yaml' 36 | os.environ['FILE_PATH'] = str(parameters_file_dir) 37 | 38 | #*****test_ekf_localization_node_interfaces.test***** 39 | ekf_node = launch_ros.actions.Node( 40 | package='robot_localization', 41 | executable='ekf_node', 42 | name='test_ekf_localization_node_interfaces_ekf', 43 | output='screen', 44 | parameters=[ 45 | parameters_file_path, 46 | str(parameters_file_path), 47 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_interfaces.yaml'], 48 | ],) 49 | 50 | return LaunchDescription([ 51 | ekf_node, 52 | ]) 53 | 54 | 55 | def main(argv=sys.argv[1:]): 56 | ld = generate_launch_description() 57 | 58 | test1_action = ExecuteProcess( 59 | cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_ekf_localization_node_interfaces'], 60 | output='screen', 61 | ) 62 | 63 | lts = LaunchTestService() 64 | lts.add_test_action(ld, test1_action) 65 | ls = LaunchService(argv=argv) 66 | ls.include_launch_description(ld) 67 | return lts.run(ls) 68 | 69 | if __name__ == '__main__': 70 | sys.exit(main()) 71 | -------------------------------------------------------------------------------- /test/test_ekf_localization_node_interfaces.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ekf_localization_node_interfaces_ekf: 3 | ros__parameters: 4 | debug: false 5 | debug_out_file: debug_ekf_localization.txt 6 | 7 | frequency: 30.0 8 | 9 | sensor_timeout: 1000.0 10 | 11 | odom0: /odom_input0 12 | odom1: /odom_input1 13 | odom2: /odom_input2 14 | 15 | pose0: /pose_input0 16 | pose1: /pose_input1 17 | 18 | twist0: /twist_input0 19 | 20 | imu0: /imu_input0 21 | imu1: /imu_input1 22 | imu2: /imu_input2 23 | imu3: /imu_input3 24 | 25 | odom0_config: [true, false, true, 26 | false, false, false, 27 | false, false, false, 28 | false, false, false, 29 | false, false, false] 30 | odom0_relative: false 31 | 32 | odom1_config: [true, true, true, 33 | true, true, true, 34 | false, false, false, 35 | false, false, false, 36 | false, false, false] 37 | odom1_relative: false 38 | 39 | odom2_config: [false, false, false, 40 | false, false, false, 41 | true, true, true, 42 | true, true, true, 43 | false, false, false] 44 | odom2_relative: false 45 | 46 | pose0_config: [true, false, true, 47 | false, false, false, 48 | false, false, false, 49 | false, false, false, 50 | false, false, false] 51 | pose1_config: [true, true, true, 52 | true, true, true, 53 | false, false, false, 54 | false, false, false, 55 | false, false, false] 56 | 57 | twist0_config: [false, false, false, 58 | false, false, false, 59 | true, true, true, 60 | true, true, true, 61 | false, false, false] 62 | 63 | imu0_config: [false, false, false, 64 | true, true, true, 65 | false, false, false, 66 | false, false, false, 67 | false, false, false] 68 | imu1_config: [false, false, false, 69 | false, false, false, 70 | false, false, false, 71 | true, true, true, 72 | false, false, false] 73 | imu2_config: [false, false, false, 74 | false, false, false, 75 | false, false, false, 76 | false, false, false, 77 | true, true, true] 78 | imu3_config: [false, false, false, 79 | true, true, true, 80 | false, false, false, 81 | false, false, false, 82 | false, false, false] 83 | 84 | odom1_differential: true 85 | pose1_differential: true 86 | imu3_differential: true 87 | 88 | print_diagnostics: false 89 | 90 | odom_frame: odom 91 | base_link_frame: base_link 92 | 93 | process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 94 | 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 95 | 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 96 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 97 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 98 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 101 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 102 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 103 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 104 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 105 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 106 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 107 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] 108 | -------------------------------------------------------------------------------- /test/test_filter_base.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include "robot_localization/filter_common.hpp" 41 | #include "robot_localization/filter_utilities.hpp" 42 | #include "robot_localization/filter_base.hpp" 43 | #include "robot_localization/measurement.hpp" 44 | 45 | using robot_localization::Measurement; 46 | using robot_localization::STATE_SIZE; 47 | 48 | namespace robot_localization 49 | { 50 | 51 | class FilterDerived : public FilterBase 52 | { 53 | public: 54 | rclcpp::Time val; 55 | 56 | FilterDerived() 57 | : val(0) {} 58 | 59 | void correct(const Measurement & measurement) 60 | { 61 | EXPECT_EQ(val, measurement.time_); 62 | EXPECT_EQ(measurement.topic_name_, "topic"); 63 | 64 | EXPECT_EQ(measurement.update_vector_.size(), 10u); 65 | for (size_t i = 0; i < measurement.update_vector_.size(); ++i) { 66 | EXPECT_EQ(measurement.update_vector_[i], true); 67 | } 68 | } 69 | void predict( 70 | const rclcpp::Time & /*reference_time*/, 71 | const rclcpp::Duration & /*delta*/) {} 72 | }; 73 | 74 | class FilterDerived2 : public FilterBase 75 | { 76 | public: 77 | FilterDerived2() {} 78 | 79 | void correct(const Measurement & /*measurement*/) {} 80 | 81 | void predict( 82 | const rclcpp::Time & /*reference_time*/, 83 | const rclcpp::Duration & /*delta*/) {} 84 | 85 | void processMeasurement(const Measurement & measurement) 86 | { 87 | FilterBase::processMeasurement(measurement); 88 | } 89 | }; 90 | 91 | } // namespace robot_localization 92 | 93 | TEST(FilterBaseTest, MeasurementStruct) { 94 | robot_localization::Measurement meas1; 95 | robot_localization::Measurement meas2; 96 | 97 | EXPECT_EQ(meas1.topic_name_, std::string("")); 98 | EXPECT_EQ(meas1.time_, rclcpp::Time(0)); 99 | EXPECT_EQ(meas2.time_, rclcpp::Time(0)); 100 | 101 | // Comparison test is true if the first 102 | // argument is > the second, so should 103 | // be false if they're equal. 104 | EXPECT_EQ(meas1(meas1, meas2), false); 105 | EXPECT_EQ(meas2(meas2, meas1), false); 106 | 107 | builtin_interfaces::msg::Time msg1; 108 | msg1.sec = 0; 109 | msg1.nanosec = 100; 110 | 111 | builtin_interfaces::msg::Time msg2; 112 | msg2.sec = 0; 113 | msg2.nanosec = 200; 114 | 115 | meas1.time_ = msg1; 116 | meas2.time_ = msg2; 117 | 118 | EXPECT_EQ(meas1(meas1, meas2), false); 119 | EXPECT_EQ(meas1(meas2, meas1), true); 120 | EXPECT_EQ(meas2(meas1, meas2), false); 121 | EXPECT_EQ(meas2(meas2, meas1), true); 122 | } 123 | 124 | TEST(FilterBaseTest, DerivedFilterGetSet) { 125 | robot_localization::FilterDerived derived; 126 | 127 | // With the ostream argument as NULL, 128 | // the debug flag will remain false. 129 | derived.setDebug(true); 130 | 131 | EXPECT_FALSE(derived.getDebug()); 132 | 133 | // Now set the stream and do it again 134 | std::stringstream os; 135 | derived.setDebug(true, &os); 136 | 137 | EXPECT_TRUE(derived.getDebug()); 138 | 139 | // Simple get/set checks 140 | double timeout = 7.4; 141 | derived.setSensorTimeout(rclcpp::Duration::from_seconds(timeout)); 142 | EXPECT_EQ(derived.getSensorTimeout(), rclcpp::Duration::from_seconds(timeout)); 143 | 144 | double lastMeasTime = 3.83; 145 | derived.setLastMeasurementTime(rclcpp::Time(lastMeasTime)); 146 | EXPECT_EQ(derived.getLastMeasurementTime(), rclcpp::Time(lastMeasTime)); 147 | 148 | Eigen::MatrixXd pnCovar(STATE_SIZE, STATE_SIZE); 149 | for (size_t i = 0; i < STATE_SIZE; ++i) { 150 | for (size_t j = 0; j < STATE_SIZE; ++j) { 151 | pnCovar(i, j) = static_cast(i * j); 152 | } 153 | } 154 | derived.setProcessNoiseCovariance(pnCovar); 155 | EXPECT_EQ(derived.getProcessNoiseCovariance(), pnCovar); 156 | 157 | Eigen::VectorXd state(STATE_SIZE); 158 | state.setZero(); 159 | derived.setState(state); 160 | EXPECT_EQ(derived.getState(), state); 161 | 162 | EXPECT_EQ(derived.getInitializedStatus(), false); 163 | } 164 | 165 | TEST(FilterBaseTest, MeasurementProcessing) { 166 | robot_localization::FilterDerived2 derived; 167 | 168 | Measurement meas; 169 | 170 | Eigen::VectorXd measurement(STATE_SIZE); 171 | for (size_t i = 0; i < STATE_SIZE; ++i) { 172 | measurement[i] = 0.1 * static_cast(i); 173 | } 174 | 175 | Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); 176 | for (size_t i = 0; i < STATE_SIZE; ++i) { 177 | for (size_t j = 0; j < STATE_SIZE; ++j) { 178 | measurementCovariance(i, j) = 0.1 * static_cast(i * j); 179 | } 180 | } 181 | 182 | builtin_interfaces::msg::Time msg; 183 | msg.sec = 0; 184 | msg.nanosec = 1000; 185 | 186 | meas.topic_name_ = "odomTest"; 187 | meas.measurement_ = measurement; 188 | meas.covariance_ = measurementCovariance; 189 | meas.update_vector_.resize(STATE_SIZE, true); 190 | meas.time_ = msg; 191 | 192 | // The filter shouldn't be initializedyet 193 | EXPECT_FALSE(derived.getInitializedStatus()); 194 | 195 | derived.processMeasurement(meas); 196 | 197 | // Now it's initialized, and the entire filter state 198 | // should be equal to the first state 199 | EXPECT_TRUE(derived.getInitializedStatus()); 200 | EXPECT_EQ(derived.getState(), measurement); 201 | 202 | msg.nanosec = 1002; 203 | // Process a measurement and make sure it updates the 204 | // lastMeasurementTime variable 205 | meas.time_ = msg; 206 | derived.processMeasurement(meas); 207 | EXPECT_EQ(derived.getLastMeasurementTime(), meas.time_); 208 | } 209 | 210 | int main(int argc, char ** argv) 211 | { 212 | testing::InitGoogleTest(&argc, argv); 213 | return RUN_ALL_TESTS(); 214 | } 215 | -------------------------------------------------------------------------------- /test/test_filter_base_diagnostics_timestamps.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2018 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from launch import LaunchDescription 17 | import launch_ros.actions 18 | import os 19 | import yaml 20 | import sys 21 | from launch.substitutions import EnvironmentVariable 22 | import pathlib 23 | import launch.actions 24 | from launch.actions import DeclareLaunchArgument 25 | from launch_testing.legacy import LaunchTestService 26 | from launch.actions import ExecuteProcess 27 | from launch import LaunchService 28 | from ament_index_python.packages import get_package_prefix 29 | 30 | 31 | def generate_launch_description(): 32 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 33 | parameters_file_path = parameters_file_dir / 'test_filter_base_diagnostics_timestamps.yaml' 34 | os.environ['FILE_PATH'] = str(parameters_file_dir) 35 | 36 | ekf_node = launch_ros.actions.Node( 37 | package='robot_localization', 38 | executable='ekf_node', 39 | name='test_filter_base_diagnostics_timestamps', 40 | output='screen', 41 | parameters=[ 42 | parameters_file_path, 43 | str(parameters_file_path), 44 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_filter_base_diagnostics_timestamps.yaml'], 45 | ],) 46 | 47 | return LaunchDescription([ 48 | ekf_node, 49 | ]) 50 | 51 | def main(argv=sys.argv[1:]): 52 | ld = generate_launch_description() 53 | 54 | test1_action = ExecuteProcess( 55 | cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_filter_base_diagnostics_timestamps'], 56 | output='screen', 57 | ) 58 | 59 | lts = LaunchTestService() 60 | lts.add_test_action(ld, test1_action) 61 | ls = LaunchService(argv=argv) 62 | ls.include_launch_description(ld) 63 | return lts.run(ls) 64 | 65 | if __name__ == '__main__': 66 | sys.exit(main()) 67 | -------------------------------------------------------------------------------- /test/test_filter_base_diagnostics_timestamps.yaml: -------------------------------------------------------------------------------- 1 | # 2 | 3 | # 4 | test_filter_base_diagnostics_timestamps: 5 | ros__parameters: 6 | debug: true 7 | debug_out_file: debug_ekf_localization.txt 8 | odom0: /example/odom 9 | pose0: /example/pose 10 | twist0: /example/twist 11 | imu0: /example/imu/data 12 | 13 | odom0_config: [false, false, false, 14 | false, false, false, 15 | true, false, false, 16 | false, false, false, 17 | false, false, false] 18 | odom0_relative: false 19 | 20 | pose0_config: [true, true, false, 21 | false, false, false, 22 | false, false, false, 23 | false, false, false, 24 | false, false, false] 25 | 26 | twist0_config: [false, false, false, 27 | false, false, false, 28 | true, true, true, 29 | true, true, true, 30 | false, false, false] 31 | 32 | imu0_config: [false, false, false, 33 | true, true, true, 34 | false, false, false, 35 | true, true, true, 36 | true, true, true] 37 | 38 | print_diagnostics: true 39 | -------------------------------------------------------------------------------- /test/test_localization_node_bag_pose_tester.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include // for bind() 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include "rclcpp/rclcpp.hpp" 44 | 45 | // using namespace std; 46 | 47 | nav_msgs::msg::Odometry filtered_; 48 | 49 | using namespace std::chrono_literals; 50 | 51 | void filterCallback(const nav_msgs::msg::Odometry::SharedPtr msg) 52 | { 53 | filtered_ = *msg; 54 | } 55 | 56 | TEST(BagTest, PoseCheck) { 57 | // node handle is created as per ros2 58 | auto node = rclcpp::Node::make_shared("localization_node_bag_pose_tester"); 59 | 60 | // getting parameters value from yaml file using get_parameter() API 61 | double finalX = node->declare_parameter("final_x", 0.0); 62 | double finalY = node->declare_parameter("final_y", 0.0); 63 | double finalZ = node->declare_parameter("final_z", 0.0); 64 | double tolerance = node->declare_parameter("tolerance", 0.0); 65 | bool outputFinalPosition = node->declare_parameter("output_final_position", false); 66 | std::string finalPositionFile = node->declare_parameter( 67 | "output_location", 68 | std::string("test.txt")); 69 | 70 | // subscribe call has been changed as per ros2 71 | auto filteredSub = node->create_subscription( 72 | "/odometry/filtered", rclcpp::QoS(1), filterCallback); 73 | 74 | // changed the spinning and timing as per ros2 75 | while (rclcpp::ok()) { 76 | rclcpp::spin_some(node); 77 | rclcpp::Rate(3).sleep(); 78 | } 79 | 80 | if (outputFinalPosition) { 81 | try { 82 | std::ofstream posOut; 83 | posOut.open(finalPositionFile.c_str(), std::ofstream::app); 84 | posOut << filtered_.pose.pose.position.x << " " << 85 | filtered_.pose.pose.position.y << " " << 86 | filtered_.pose.pose.position.z << std::endl; 87 | posOut.close(); 88 | } catch (...) { 89 | RCLCPP_ERROR(node->get_logger(), "Unable to open output file.\n"); 90 | } 91 | } 92 | 93 | double xDiff = filtered_.pose.pose.position.x - finalX; 94 | double yDiff = filtered_.pose.pose.position.y - finalY; 95 | double zDiff = filtered_.pose.pose.position.z - finalZ; 96 | 97 | std::cout << "xDiff =" << xDiff << std::endl; 98 | std::cout << "yDiff =" << yDiff << std::endl; 99 | std::cout << "zDiff =" << zDiff << std::endl; 100 | 101 | EXPECT_LT(::sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff), tolerance); 102 | } 103 | 104 | int main(int argc, char ** argv) 105 | { 106 | rclcpp::init(argc, argv); 107 | ::testing::InitGoogleTest(&argc, argv); 108 | rclcpp::Rate(0.5).sleep(); 109 | 110 | int ret = RUN_ALL_TESTS(); 111 | rclcpp::shutdown(); 112 | return ret; 113 | } 114 | -------------------------------------------------------------------------------- /test/test_robot_localization_estimator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include "robot_localization/robot_localization_estimator.hpp" 39 | 40 | TEST(RLETest, StateBuffer) 41 | { 42 | // Generate a few empty estimator states 43 | std::vector states; 44 | 45 | for (int i = 0; i < 10; i++) { 46 | /* 47 | * t = i s; 48 | * x = i m; 49 | * vx = 1.0 m/s; 50 | */ 51 | robot_localization::EstimatorState state; 52 | state.time_stamp = i; 53 | state.state(robot_localization::StateMemberX) = i; 54 | state.state(robot_localization::StateMemberY) = 0; 55 | state.state(robot_localization::StateMemberZ) = 0; 56 | 57 | state.state(robot_localization::StateMemberRoll) = 0; 58 | state.state(robot_localization::StateMemberPitch) = 0; 59 | state.state(robot_localization::StateMemberYaw) = 0; 60 | 61 | state.state(robot_localization::StateMemberVx) = 1; 62 | state.state(robot_localization::StateMemberVy) = 0; 63 | state.state(robot_localization::StateMemberVz) = 0; 64 | 65 | state.state(robot_localization::StateMemberVroll) = 0; 66 | state.state(robot_localization::StateMemberVpitch) = 0; 67 | state.state(robot_localization::StateMemberVyaw) = 0; 68 | 69 | state.state(robot_localization::StateMemberAx) = 0; 70 | state.state(robot_localization::StateMemberAy) = 0; 71 | state.state(robot_localization::StateMemberAz) = 0; 72 | states.push_back(state); 73 | } 74 | 75 | // Instantiate a robot localization estimator with a buffer capacity of 5 76 | unsigned int buffer_capacity = 5; 77 | Eigen::MatrixXd process_noise_covariance = Eigen::MatrixXd::Identity( 78 | robot_localization::STATE_SIZE, robot_localization::STATE_SIZE); 79 | robot_localization::RobotLocalizationEstimator estimator(buffer_capacity, 80 | robot_localization::FilterTypes::EKF, process_noise_covariance); 81 | 82 | robot_localization::EstimatorState state; 83 | 84 | // Add the states in chronological order 85 | for (int i = 0; i < 6; i++) { 86 | estimator.setState(states[i]); 87 | 88 | // Check that the state is added correctly 89 | estimator.getState(states[i].time_stamp, state); 90 | EXPECT_EQ(state.time_stamp, states[i].time_stamp); 91 | } 92 | 93 | // We filled the buffer with more states that it can hold, so its size should 94 | // now be equal to the capacity 95 | EXPECT_EQ(static_cast(estimator.getSize()), buffer_capacity); 96 | 97 | // Clear the buffer and check if it's really empty afterwards 98 | estimator.clearBuffer(); 99 | EXPECT_EQ(estimator.getSize(), 0u); 100 | 101 | // Add states at time 1 through 3 inclusive to the buffer (buffer is not yet 102 | // full) 103 | for (int i = 1; i < 4; i++) { 104 | estimator.setState(states[i]); 105 | } 106 | 107 | // Now add a state at time 0, but let's change it a bit (set StateMemberY=12) 108 | // so that we can inspect if it is correctly added to the buffer. 109 | robot_localization::EstimatorState state_2 = states[0]; 110 | state_2.state(robot_localization::StateMemberY) = 12; 111 | estimator.setState(state_2); 112 | EXPECT_EQ( 113 | robot_localization::EstimatorResults::Exact, 114 | estimator.getState(states[0].time_stamp, state)); 115 | 116 | // Check if the state is correctly added 117 | EXPECT_EQ(state.state, state_2.state); 118 | 119 | // Add some more states. State at t=0 should now be dropped, so we should get 120 | // the prediction, which means y=0 121 | for (int i = 5; i < 8; i++) { 122 | estimator.setState(states[i]); 123 | } 124 | EXPECT_EQ( 125 | robot_localization::EstimatorResults::ExtrapolationIntoPast, 126 | estimator.getState(states[0].time_stamp, state)); 127 | EXPECT_EQ(states[0].state, state.state); 128 | 129 | // Estimate a state that is not in the buffer, but can be determined by 130 | // interpolation. The predicted state vector should be equal to the designed 131 | // state at the requested time. 132 | EXPECT_EQ( 133 | robot_localization::EstimatorResults::Interpolation, 134 | estimator.getState(states[4].time_stamp, state)); 135 | EXPECT_EQ(states[4].state, state.state); 136 | 137 | // Estimate a state that is not in the buffer, but can be determined by 138 | // extrapolation into the future. The predicted state vector should be equal 139 | // to the designed state at the requested time. 140 | EXPECT_EQ( 141 | robot_localization::EstimatorResults::ExtrapolationIntoFuture, 142 | estimator.getState(states[8].time_stamp, state)); 143 | EXPECT_EQ(states[8].state, state.state); 144 | 145 | // Add missing state somewhere in the middle 146 | estimator.setState(states[4]); 147 | 148 | // Overwrite state at t=3 (oldest state now in the buffer) and check if it's 149 | // correctly overwritten. 150 | state_2 = states[3]; 151 | state_2.state(robot_localization::StateMemberVy) = -1.0; 152 | estimator.setState(state_2); 153 | EXPECT_EQ( 154 | robot_localization::EstimatorResults::Exact, 155 | estimator.getState(states[3].time_stamp, state)); 156 | EXPECT_EQ(state_2.state, state.state); 157 | 158 | // Add state that came too late 159 | estimator.setState(states[0]); 160 | 161 | // Check if getState needed to do extrapolation into the past 162 | EXPECT_EQ( 163 | estimator.getState(states[0].time_stamp, state), 164 | robot_localization::EstimatorResults::ExtrapolationIntoPast); 165 | 166 | // Check state at t=0. This can only work correctly if the state at t=3 is 167 | // overwritten and the state at zero is not in the buffer. 168 | EXPECT_DOUBLE_EQ(3.0, state.state(robot_localization::StateMemberY)); 169 | } 170 | 171 | int main(int argc, char ** argv) 172 | { 173 | rclcpp::init(argc, argv); 174 | auto node = rclcpp::Node::make_shared("test_robot_localization_estimator"); 175 | 176 | testing::InitGoogleTest(&argc, argv); 177 | return RUN_ALL_TESTS(); 178 | } 179 | -------------------------------------------------------------------------------- /test/test_robot_localization_estimator.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import ament_index_python.packages 4 | import launch 5 | import launch_ros.actions 6 | 7 | def generate_launch_description(): 8 | 9 | # TODO: Port ROS 1 test launch params: clear_params="true" 10 | return launch.LaunchDescription([ 11 | launch_ros.actions.Node( 12 | package='robot_localization', 13 | executable='test_robot_localization_estimator', 14 | name='test_rle', 15 | ), 16 | ]) 17 | -------------------------------------------------------------------------------- /test/test_ros_robot_localization_listener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, TNO IVS, Helmond 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include "robot_localization/ros_robot_localization_listener.hpp" 41 | #include "robot_localization/filter_common.hpp" 42 | 43 | std::shared_ptr node; 44 | std::unique_ptr g_listener; 45 | 46 | TEST(LocalizationListenerTest, testGetStateOfBaseLink) 47 | { 48 | rclcpp::spin_some(node); 49 | 50 | rclcpp::Time time2(1001, 0); 51 | 52 | Eigen::VectorXd state(robot_localization::STATE_SIZE); 53 | Eigen::MatrixXd covariance(robot_localization::STATE_SIZE, robot_localization::STATE_SIZE); 54 | 55 | 56 | std::string base_frame("base_link"); 57 | g_listener->getState(time2, base_frame, state, covariance); 58 | 59 | EXPECT_DOUBLE_EQ(1.0, state(robot_localization::StateMemberX)); 60 | EXPECT_DOUBLE_EQ(0.0, state(robot_localization::StateMemberY)); 61 | EXPECT_DOUBLE_EQ(0.0, state(robot_localization::StateMemberZ)); 62 | 63 | EXPECT_FLOAT_EQ(M_PI / 4, state(robot_localization::StateMemberRoll)); 64 | EXPECT_FLOAT_EQ(0.0, state(robot_localization::StateMemberPitch)); 65 | EXPECT_FLOAT_EQ(0.0, state(robot_localization::StateMemberYaw)); 66 | 67 | EXPECT_DOUBLE_EQ(M_PI / 4.0, state(robot_localization::StateMemberVroll)); 68 | EXPECT_DOUBLE_EQ(0.0, state(robot_localization::StateMemberVpitch)); 69 | EXPECT_DOUBLE_EQ(0.0, state(robot_localization::StateMemberVyaw)); 70 | } 71 | 72 | TEST(LocalizationListenerTest, GetStateOfRelatedFrame) 73 | { 74 | rclcpp::spin_some(node); 75 | 76 | Eigen::VectorXd state(robot_localization::STATE_SIZE); 77 | Eigen::MatrixXd covariance(robot_localization::STATE_SIZE, robot_localization::STATE_SIZE); 78 | 79 | rclcpp::Time time1(1000, 0); 80 | rclcpp::Time time2(1001, 0); 81 | 82 | std::string sensor_frame("sensor"); 83 | 84 | EXPECT_TRUE(g_listener->getState(time1, sensor_frame, state, covariance) ); 85 | 86 | EXPECT_FLOAT_EQ(0.0, state(robot_localization::StateMemberX)); 87 | EXPECT_FLOAT_EQ(1.0, state(robot_localization::StateMemberY)); 88 | EXPECT_FLOAT_EQ(0.0, state(robot_localization::StateMemberZ)); 89 | 90 | EXPECT_FLOAT_EQ(0.0, state(robot_localization::StateMemberRoll)); 91 | EXPECT_FLOAT_EQ(0.0, state(robot_localization::StateMemberPitch)); 92 | EXPECT_FLOAT_EQ(M_PI / 2, state(robot_localization::StateMemberYaw)); 93 | 94 | EXPECT_TRUE(1e-12 > state(robot_localization::StateMemberVx)); 95 | EXPECT_FLOAT_EQ(-1.0, state(robot_localization::StateMemberVy)); 96 | EXPECT_FLOAT_EQ(M_PI / 4.0, state(robot_localization::StateMemberVz)); 97 | 98 | EXPECT_TRUE(1e-12 > state(robot_localization::StateMemberVroll)); 99 | EXPECT_FLOAT_EQ(-M_PI / 4.0, state(robot_localization::StateMemberVpitch)); 100 | EXPECT_FLOAT_EQ(0.0, state(robot_localization::StateMemberVyaw)); 101 | 102 | EXPECT_TRUE(g_listener->getState(time2, sensor_frame, state, covariance)); 103 | 104 | EXPECT_FLOAT_EQ(1.0, state(robot_localization::StateMemberX)); 105 | EXPECT_FLOAT_EQ(sqrt(2) / 2.0, state(robot_localization::StateMemberY)); 106 | EXPECT_FLOAT_EQ(sqrt(2) / 2.0, state(robot_localization::StateMemberZ)); 107 | 108 | EXPECT_TRUE(1e-12 > state(robot_localization::StateMemberRoll)); 109 | EXPECT_TRUE(1e-12 > fabs(-M_PI / 4.0 - state(robot_localization::StateMemberPitch))); 110 | EXPECT_FLOAT_EQ(M_PI / 2, state(robot_localization::StateMemberYaw)); 111 | 112 | EXPECT_TRUE(1e-12 > state(robot_localization::StateMemberVx)); 113 | EXPECT_FLOAT_EQ(-1.0, state(robot_localization::StateMemberVy)); 114 | EXPECT_FLOAT_EQ(M_PI / 4, state(robot_localization::StateMemberVz)); 115 | 116 | EXPECT_TRUE(1e-12 > state(robot_localization::StateMemberVroll)); 117 | EXPECT_FLOAT_EQ(-M_PI / 4.0, state(robot_localization::StateMemberVpitch)); 118 | EXPECT_FLOAT_EQ(0, state(robot_localization::StateMemberVyaw)); 119 | } 120 | 121 | int main(int argc, char ** argv) 122 | { 123 | rclcpp::init(argc, argv); 124 | node = rclcpp::Node::make_shared("test_ros_robot_localization_listener"); 125 | 126 | g_listener = std::make_unique(node); 127 | 128 | ::testing::InitGoogleTest(&argc, argv); 129 | 130 | int res = RUN_ALL_TESTS(); 131 | 132 | rclcpp::shutdown(); 133 | node = nullptr; 134 | g_listener = nullptr; 135 | 136 | return res; 137 | } 138 | -------------------------------------------------------------------------------- /test/test_ros_robot_localization_listener.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import ament_index_python.packages 4 | import launch 5 | import launch_ros.actions 6 | 7 | import os 8 | 9 | def generate_launch_description(): 10 | 11 | default_params_yaml = os.path.join( 12 | ament_index_python.packages.get_package_share_directory('robot_localization'), 13 | 'test', 'test_ros_robot_localization_listener.yaml') 14 | 15 | return launch.LaunchDescription([ 16 | # TODO: Port ROS 1 test launch params: clear_params="true" 17 | launch_ros.actions.Node( 18 | package='robot_localization', 19 | executable='test_ros_robot_localization_listener', 20 | remappings=[('test_ros_robot_localization_listener', 'test_estimator')], 21 | arguments=['__params:=' + default_params_yaml], 22 | output='screen', 23 | ), 24 | # TODO: Port ROS 1 test launch params: clear_params="true" 25 | launch_ros.actions.Node( 26 | package='robot_localization', 27 | executable='test_ros_robot_localization_listener_publisher', 28 | name='test_estimator', 29 | remappings=[ 30 | ('/odometry/filtered', 'odom/filtered'), 31 | ('/accel/filtered', 'acceleration/filtered'), 32 | ], 33 | output='screen', 34 | ), 35 | ]) 36 | -------------------------------------------------------------------------------- /test/test_ros_robot_localization_listener.yaml: -------------------------------------------------------------------------------- 1 | test_ros_robot_localization_listener: 2 | ros__parameters: 3 | frequency: 30 4 | sensor_timeout: 0.1 5 | odom0: /odom_input0 6 | odom1: /odom_input1 7 | odom2: /odom_input2 8 | pose0: /pose_input0 9 | pose1: /pose_input1 10 | twist0: /twist_input0 11 | imu0: /imu_input0 12 | imu1: /imu_input1 13 | imu2: /imu_input2 14 | imu3: /imu_input3 15 | odom0_config: [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] 16 | odom1_config: [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] 17 | odom2_config: [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] 18 | pose0_config: [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] 19 | pose1_config: [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] 20 | twist0_config: [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] 21 | imu0_config: [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] 22 | imu1_config: [false, false, false, false, false, false, false, false, false, true, true, true, false, false, false] 23 | imu2_config: [false, false, false, false, false, false, false, false, false, false, false, false, true, true, true] 24 | imu3_config: [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] 25 | odom1_differential: true 26 | pose1_differential: true 27 | imu3_differential: true 28 | print_diagnostics: false 29 | odom_frame: odom 30 | base_link_frame: base_link 31 | process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 32 | 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 33 | 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 34 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 37 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 38 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 39 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 44 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 45 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] 46 | -------------------------------------------------------------------------------- /test/test_ros_robot_localization_listener_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, TNO IVS, Helmond 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | int main(int argc, char ** argv) 43 | { 44 | rclcpp::init(argc, argv); 45 | std::shared_ptr node = rclcpp::Node::make_shared( 46 | "test_robot_localization_listener_publisher"); 47 | 48 | rclcpp::Publisher::SharedPtr odom_pub = 49 | node->create_publisher("odometry/filtered", 1); 50 | rclcpp::Publisher::SharedPtr accel_pub = 51 | node->create_publisher("accel/filtered", 1); 52 | 53 | tf2_ros::StaticTransformBroadcaster transform_broadcaster(node); 54 | 55 | rclcpp::Time end_time = node->now() + rclcpp::Duration(10, 0); 56 | while (rclcpp::ok() && node->now() < end_time) { 57 | rclcpp::Time time1(1000, 0); 58 | double x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az; 59 | x = y = z = roll = pitch = yaw = vy = vz = vroll = vpitch = vyaw = ax = ay = az = 0.0; 60 | vx = 1.0; 61 | vroll = M_PI / 4.0; 62 | 63 | tf2::Quaternion q; 64 | q.setRPY(0, 0, 0); 65 | 66 | nav_msgs::msg::Odometry odom_msg; 67 | odom_msg.header.stamp = time1; 68 | odom_msg.header.frame_id = "map"; 69 | odom_msg.child_frame_id = "base_link"; 70 | odom_msg.pose.pose.position.x = x; 71 | odom_msg.pose.pose.position.y = y; 72 | odom_msg.pose.pose.position.y = z; 73 | odom_msg.pose.pose.orientation.x = q.x(); 74 | odom_msg.pose.pose.orientation.y = q.y(); 75 | odom_msg.pose.pose.orientation.z = q.z(); 76 | odom_msg.pose.pose.orientation.w = q.w(); 77 | 78 | odom_msg.twist.twist.linear.x = vx; 79 | odom_msg.twist.twist.linear.y = vy; 80 | odom_msg.twist.twist.linear.z = vz; 81 | odom_msg.twist.twist.angular.x = vroll; 82 | odom_msg.twist.twist.angular.y = vpitch; 83 | odom_msg.twist.twist.angular.z = vyaw; 84 | 85 | geometry_msgs::msg::AccelWithCovarianceStamped accel_msg; 86 | accel_msg.header.stamp = time1; 87 | accel_msg.header.frame_id = "base_link"; 88 | accel_msg.accel.accel.linear.x = ax; 89 | accel_msg.accel.accel.linear.y = ay; 90 | accel_msg.accel.accel.linear.z = az; 91 | accel_msg.accel.accel.angular.x = 0; 92 | accel_msg.accel.accel.angular.y = 0; 93 | accel_msg.accel.accel.angular.z = 0; 94 | 95 | odom_pub->publish(odom_msg); 96 | accel_pub->publish(accel_msg); 97 | 98 | geometry_msgs::msg::TransformStamped transformStamped; 99 | 100 | transformStamped.header.stamp = node->now(); 101 | transformStamped.header.frame_id = "base_link"; 102 | transformStamped.child_frame_id = "sensor"; 103 | transformStamped.transform.translation.x = 0.0; 104 | transformStamped.transform.translation.y = 1.0; 105 | transformStamped.transform.translation.z = 0.0; 106 | { 107 | tf2::Quaternion q; 108 | q.setRPY(0, 0, M_PI / 2); 109 | transformStamped.transform.rotation.x = q.x(); 110 | transformStamped.transform.rotation.y = q.y(); 111 | transformStamped.transform.rotation.z = q.z(); 112 | transformStamped.transform.rotation.w = q.w(); 113 | 114 | transform_broadcaster.sendTransform(transformStamped); 115 | } 116 | 117 | rclcpp::Rate(10).sleep(); 118 | } 119 | 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /test/test_ukf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * 3. Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include "robot_localization/ros_filter_types.hpp" 39 | #include "robot_localization/ros_filter.hpp" 40 | #include "robot_localization/ukf.hpp" 41 | 42 | using robot_localization::STATE_SIZE; 43 | using robot_localization::Ukf; 44 | using robot_localization::RosUkf; 45 | 46 | TEST(UkfTest, Measurements) { 47 | // node handle is created as per ros2 48 | rclcpp::NodeOptions options; 49 | options.arguments({"ukf_test_node"}); 50 | std::shared_ptr filter = 51 | std::make_shared(options); 52 | filter->initialize(); 53 | double alpha = filter->declare_parameter("alpha", 0.001); 54 | double kappa = filter->declare_parameter("kappa", 0.0); 55 | double beta = filter->declare_parameter("beta", 2.0); 56 | filter->getFilter().setConstants(alpha, kappa, beta); 57 | 58 | // create the instance of the class and pass parameters 59 | Eigen::MatrixXd initialCovar(15, 15); 60 | 61 | initialCovar.setIdentity(); 62 | initialCovar *= 0.5; 63 | 64 | filter->getFilter().setEstimateErrorCovariance(initialCovar); 65 | 66 | EXPECT_EQ(filter->getFilter().getEstimateErrorCovariance(), initialCovar); 67 | 68 | Eigen::VectorXd measurement(STATE_SIZE); 69 | measurement.setIdentity(); 70 | 71 | for (size_t i = 0; i < STATE_SIZE; ++i) { 72 | measurement[i] = i * 0.01 * STATE_SIZE; 73 | } 74 | Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); 75 | measurementCovariance.setIdentity(); 76 | for (size_t i = 0; i < STATE_SIZE; ++i) { 77 | measurementCovariance(i, i) = 1e-9; 78 | } 79 | std::vector updateVector(STATE_SIZE, true); 80 | 81 | // Ensure that measurements are being placed in the queue correctly 82 | rclcpp::Time time1(1000); 83 | filter->robot_localization::RosUkf::enqueueMeasurement( 84 | "odom0", measurement, measurementCovariance, updateVector, 85 | std::numeric_limits::max(), time1); 86 | 87 | filter->robot_localization::RosUkf::integrateMeasurements(rclcpp::Time(1001)); 88 | 89 | EXPECT_EQ(filter->getFilter().getState(), measurement); 90 | EXPECT_EQ( 91 | filter->getFilter().getEstimateErrorCovariance(), 92 | measurementCovariance); 93 | 94 | filter->getFilter().setEstimateErrorCovariance(initialCovar); 95 | 96 | // Now fuse another measurement and check the output. 97 | // We know what the filter's state should be when 98 | // this is complete, so we'll check the difference and 99 | // make sure it's suitably small. 100 | Eigen::VectorXd measurement2 = measurement; 101 | 102 | measurement2 *= 2.0; 103 | 104 | for (size_t i = 0; i < STATE_SIZE; ++i) { 105 | measurementCovariance(i, i) = 1e-9; 106 | } 107 | 108 | rclcpp::Time time2(1002); 109 | 110 | filter->robot_localization::RosUkf::enqueueMeasurement( 111 | "odom0", measurement2, measurementCovariance, updateVector, 112 | std::numeric_limits::max(), time2); 113 | 114 | filter->robot_localization::RosUkf::integrateMeasurements(rclcpp::Time(1003)); 115 | 116 | measurement = measurement2.eval() - filter->getFilter().getState(); 117 | for (size_t i = 0; i < STATE_SIZE; ++i) { 118 | EXPECT_LT(::fabs(measurement[i]), 0.001); 119 | } 120 | } 121 | 122 | int main(int argc, char ** argv) 123 | { 124 | rclcpp::init(argc, argv); 125 | ::testing::InitGoogleTest(&argc, argv); 126 | int ret = RUN_ALL_TESTS(); 127 | rclcpp::shutdown(); 128 | 129 | return ret; 130 | } 131 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag1.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | import launch_ros.actions 17 | import os 18 | import yaml 19 | from launch.substitutions import EnvironmentVariable 20 | import pathlib 21 | import launch.actions 22 | from launch.actions import DeclareLaunchArgument 23 | 24 | 25 | def generate_launch_description(): 26 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 27 | parameters_file_path = parameters_file_dir / 'test_ukf_localization_node_bag1.yaml' 28 | os.environ['FILE_PATH'] = str(parameters_file_dir) 29 | return LaunchDescription([ 30 | launch.actions.DeclareLaunchArgument( 31 | 'output_final_position', 32 | default_value='false'), 33 | launch.actions.DeclareLaunchArgument( 34 | 'output_location', 35 | default_value='ukf1.txt'), 36 | 37 | #launch_ros.actions.Node( 38 | # package='tf2_ros', executable='static_transform_publisher',node_name='bl_imu', output='screen', 39 | # arguments=['0', '-0.3', '0.52', '-1.570796327', '0', '1.570796327', 'base_link', 'imu_link'] 40 | # ), 41 | 42 | launch_ros.actions.Node( 43 | package='robot_localization', executable='ukf_node', name='test_ukf_localization_node_bag1_ukf', 44 | output='screen', 45 | parameters=[ 46 | parameters_file_path, 47 | str(parameters_file_path), 48 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_bag1.yaml'], 49 | ], 50 | ), 51 | 52 | launch_ros.actions.Node( 53 | package='robot_localization', executable='test_ukf_localization_node_bag1', name='test_ukf_localization_node_bag1_pose', 54 | output='screen', 55 | parameters=[ 56 | parameters_file_path, 57 | str(parameters_file_path), 58 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_bag1.yaml'], 59 | ], 60 | ), 61 | ]) 62 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #************************ test_ukf_localization_node_bag1 *************************** 4 | 5 | # In this automatic Script, multiple terminals will be opened, user need to check the terminal on which bag is playing. Once bag is stoped, check the terminal on which launch.py is running, user will see, test cases result (Pass Or Fail) after pressing ctrl+c. 6 | 7 | $PWD = `pwd` 8 | echo "Current Working Directory = $PWD" 9 | ROS1_DISTRO=melodic 10 | ROS2_DISTRO=crystal 11 | echo "ROS1_DISTRO = $ROS1_DISTRO" 12 | echo "ROS2_DISTRO = $ROS2_DISTRO" 13 | 14 | #Command to run roscore 15 | cmd1="source /opt/ros/$ROS1_DISTRO/setup.sh; roscore; exec /bin/bash" 16 | 17 | #Command to run ros1_bridge 18 | cmd2="source ~/ros2_ws/install/setup.bash; source /opt/ros/$ROS1_DISTRO/setup.bash; ros2 run ros1_bridge dynamic_bridge --bridge-all-topics; exec /bin/bash" 19 | 20 | #Command to play .bag from ROS1 21 | cmd3="source /opt/ros/$ROS1_DISTRO/setup.bash; rosparam set use_sim_time true; rosbag play $PWD/src/robot_localization/test/test1.bag --clock -d 5; exec /bin/bash" 22 | 23 | #Command to launch TestCase launch.py 24 | cmd4="source /opt/ros/$ROS2_DISTRO/setup.bash; source $PWD/install/setup.bash; ros2 launch robot_localization test_ukf_localization_node_bag1.launch.py; exec /bin/bash" 25 | 26 | #Command to run static_transform_publisher 27 | cmd5="source /opt/ros/$ROS2_DISTRO/setup.bash; ros2 run tf2_ros static_transform_publisher 0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link; exec /bin/bash" 28 | 29 | gnome-terminal --tab -t "roscore" -- /bin/bash -c "$cmd1" 30 | sleep 1 31 | gnome-terminal --tab -t "ros1_bridge" -- /bin/bash -c "$cmd2" 32 | gnome-terminal --tab -t "bag" -- /bin/bash -c "$cmd3" 33 | gnome-terminal --tab -t "TestCase_launch" -- /bin/bash -c "$cmd4" 34 | gnome-terminal --tab -t "static_transform_publisher" -- /bin/bash -c "$cmd5" 35 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag1.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ukf_localization_node_bag1_ukf: 3 | ros__parameters: 4 | use_sim_time: true 5 | clear_params: true 6 | filter_type: "ukf" 7 | frequency: 30.0 8 | sensor_timeout: 0.1 9 | two_d_mode: false 10 | map_frame: map 11 | odom_frame: odom 12 | base_link_frame: base_link 13 | world_frame: odom 14 | transform_time_offset: 0.0 15 | transform_timeout: 0.0 16 | odom0: /husky_velocity_controller/odom 17 | imu0: /imu/data 18 | odom0_config: [false, false, false, 19 | false, false, false, 20 | true, true, true, 21 | false, false, false, 22 | false, false, false] 23 | imu0_config: [false, false, false, 24 | true, true, true, 25 | false, false, false, 26 | true, true, false, 27 | true, true, true] 28 | odom0_differential: false 29 | imu0_differential: false 30 | odom0_relative: false 31 | imu0_relative: false 32 | imu0_remove_gravitational_acceleration: true 33 | print_diagnostics: true 34 | odom0_queue_size: 10 35 | imu0_queue_size: 10 36 | debug: false 37 | debug_out_file: debug_ekf_localization.txt 38 | process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 39 | 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 44 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 45 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 46 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 48 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 49 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 50 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 51 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 52 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] 53 | 54 | initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55 | 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 56 | 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 57 | 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 58 | 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 59 | 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 60 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 61 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 62 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 63 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 64 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 65 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 66 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 67 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 68 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] 69 | 70 | alpha: 0.001 71 | kappa: 0.0 72 | beta: 2.0 73 | 74 | # 75 | test_ukf_localization_node_bag1_pose: 76 | ros__parameters: 77 | clear_params: true 78 | final_x: -39.7333 79 | final_y: -76.9820 80 | final_z: -2.4427 81 | tolerance: 1.2910 82 | output_final_position: true 83 | output_location: ukf1.txt 84 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag2.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | import launch_ros.actions 17 | import os 18 | import yaml 19 | from launch.substitutions import EnvironmentVariable 20 | import pathlib 21 | import launch.actions 22 | from launch.actions import DeclareLaunchArgument 23 | 24 | 25 | def generate_launch_description(): 26 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 27 | parameters_file_path = parameters_file_dir / 'test_ukf_localization_node_bag2.yaml' 28 | os.environ['FILE_PATH'] = str(parameters_file_dir) 29 | return LaunchDescription([ 30 | launch.actions.DeclareLaunchArgument( 31 | 'output_final_position', 32 | default_value='false'), 33 | launch.actions.DeclareLaunchArgument( 34 | 'output_location', 35 | default_value='ukf2.txt'), 36 | 37 | launch_ros.actions.Node( 38 | package='robot_localization', executable='ukf_node', name='test_ukf_localization_node_bag2_ukf', 39 | output='screen', 40 | parameters=[ 41 | parameters_file_path, 42 | str(parameters_file_path), 43 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_bag2.yaml'], 44 | ], 45 | ), 46 | launch_ros.actions.Node( 47 | package='robot_localization', executable='test_ekf_localization_node_bag1', name='test_ukf_localization_node_bag2_pose', 48 | output='screen', 49 | parameters=[ 50 | parameters_file_path, 51 | str(parameters_file_path), 52 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_bag2.yaml'], 53 | ], 54 | ), 55 | ]) 56 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag2.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #************************ test_ukf_localization_node_bag2 *************************** 4 | 5 | # In this automatic Script, multiple terminals will be opened, user need to check the terminal on which bag is playing. Once bag is stoped, check the terminal on which launch.py is running, user will see, test cases result (Pass Or Fail) after pressing ctrl+c. 6 | 7 | $PWD = `pwd` 8 | echo "Current Working Directory = $PWD" 9 | ROS1_DISTRO=melodic 10 | ROS2_DISTRO=crystal 11 | echo "ROS1_DISTRO = $ROS1_DISTRO" 12 | echo "ROS2_DISTRO = $ROS2_DISTRO" 13 | 14 | #Command to run roscore 15 | cmd1="source /opt/ros/$ROS1_DISTRO/setup.sh; roscore; exec /bin/bash" 16 | 17 | #Command to run ros1_bridge 18 | cmd2="source ~/ros2_ws/install/setup.bash; source /opt/ros/$ROS1_DISTRO/setup.bash; ros2 run ros1_bridge dynamic_bridge --bridge-all-topics; exec /bin/bash" 19 | 20 | #Command to play .bag from ROS1 21 | cmd3="source /opt/ros/$ROS1_DISTRO/setup.bash; rosparam set use_sim_time true; rosbag play $PWD/src/robot_localization/test/test2.bag --clock -d 5; exec /bin/bash" 22 | 23 | #Command to launch TestCase launch.py 24 | cmd4="source /opt/ros/$ROS2_DISTRO/setup.bash; source $PWD/install/setup.bash; ros2 launch robot_localization test_ukf_localization_node_bag2.launch.py; exec /bin/bash" 25 | 26 | gnome-terminal --tab -t "roscore" -- /bin/bash -c "$cmd1" 27 | sleep 1 28 | gnome-terminal --tab -t "bag" -- /bin/bash -c "$cmd3" 29 | gnome-terminal --tab -t "TestCase_launch" -- /bin/bash -c "$cmd4" 30 | gnome-terminal --tab -t "ros1_bridge" -- /bin/bash -c "$cmd2" 31 | 32 | 33 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag2.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ukf_localization_node_bag2_ukf: 3 | ros__parameters: 4 | use_sim_time: true 5 | clear_params: true 6 | filter_type: "ukf" 7 | frequency: 50.0 8 | sensor_timeout: 0.1 9 | odom0: /jackal_velocity_controller/odom 10 | imu0: /imu/data 11 | map_frame: map 12 | odom_frame: odom 13 | base_link_frame: base_link 14 | world_frame: odom 15 | odom0_config: [false, false, false, 16 | false, false, false, 17 | true, true, true, 18 | false, false, true, 19 | false, false, false] 20 | odom0_relative: false 21 | imu0_config: [false, false, false, 22 | true, true, true, 23 | false, false, false, 24 | true, true, true, 25 | true, true, true] 26 | odom0_queue_size: 10 27 | imu0_queue_size: 10 28 | imu0_remove_gravitational_acceleration: true 29 | process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 30 | 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 31 | 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 32 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 33 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 34 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 37 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 38 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 39 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 40 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 41 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 42 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 43 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] 44 | 45 | initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 46 | 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47 | 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 48 | 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 49 | 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50 | 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 51 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 52 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 54 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 55 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 56 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 57 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 58 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 59 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] 60 | 61 | alpha: 0.001 62 | kappa: 0.0 63 | beta: 2.0 64 | 65 | # 66 | test_ukf_localization_node_bag2_pose: 67 | ros__parameters: 68 | clear_params: true 69 | final_x: 1.0438 70 | final_y: 3.4940 71 | final_z: 0.7260 72 | tolerance: 0.2351 73 | output_final_position: true 74 | output_location: ukf2.txt 75 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag3.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | import launch_ros.actions 17 | import os 18 | import yaml 19 | from launch.substitutions import EnvironmentVariable 20 | import pathlib 21 | import launch.actions 22 | from launch.actions import DeclareLaunchArgument 23 | 24 | 25 | def generate_launch_description(): 26 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 27 | parameters_file_path = parameters_file_dir /'test_ukf_localization_node_bag3.yaml' 28 | os.environ['FILE_PATH'] = str(parameters_file_dir) 29 | return LaunchDescription([ 30 | launch.actions.DeclareLaunchArgument( 31 | 'output_final_position', 32 | default_value='false'), 33 | launch.actions.DeclareLaunchArgument( 34 | 'output_location', 35 | default_value='ukf3.txt'), 36 | 37 | #*****test_ukf_localization_node_bag3.test***** 38 | launch_ros.actions.Node( 39 | package='robot_localization', executable='ukf_node', name='test_ukf_localization_node_bag3_ukf', 40 | output='screen', 41 | parameters=[ 42 | parameters_file_path, 43 | str(parameters_file_path), 44 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_bag3.yaml'], 45 | ], 46 | ), 47 | launch_ros.actions.Node( 48 | package='robot_localization', executable='test_ukf_localization_node_bag3', name='test_ukf_localization_node_bag3_pose', 49 | output='screen', 50 | parameters=[ 51 | parameters_file_path, 52 | str(parameters_file_path), 53 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_bag3.yaml'], 54 | ], 55 | ), 56 | ]) 57 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag3.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #************************ test_ukf_localization_node_bag3 *************************** 4 | 5 | # In this automatic Script, multiple terminals will be opened, user need to check the terminal on which bag is playing. Once bag is stoped, check the terminal on which launch.py is running, user will see, test cases result (Pass Or Fail) after pressing ctrl+c. 6 | 7 | $PWD = `pwd` 8 | echo "Current Working Directory = $PWD" 9 | ROS1_DISTRO=melodic 10 | ROS2_DISTRO=crystal 11 | echo "ROS1_DISTRO = $ROS1_DISTRO" 12 | echo "ROS2_DISTRO = $ROS2_DISTRO" 13 | 14 | #Command to run roscore 15 | cmd1="source /opt/ros/$ROS1_DISTRO/setup.sh; roscore; exec /bin/bash" 16 | 17 | #Command to run ros1_bridge 18 | cmd2="source ~/ros2_ws/install/setup.bash; source /opt/ros/$ROS1_DISTRO/setup.bash; ros2 run ros1_bridge dynamic_bridge --bridge-all-topics; exec /bin/bash" 19 | 20 | #Command to play .bag from ROS1 21 | cmd3="sleep 2; source /opt/ros/$ROS1_DISTRO/setup.bash; rosparam set /use_sim_time true; rosbag play $PWD/src/robot_localization/test/test3.bag --clock -d 5; exec /bin/bash" 22 | 23 | #Command to launch TestCase launch.py 24 | cmd4="sleep 2; source /opt/ros/$ROS2_DISTRO/setup.bash; source $PWD/install/setup.bash; ros2 launch robot_localization test_ukf_localization_node_bag3.launch.py; exec /bin/bash" 25 | 26 | gnome-terminal --tab -t "roscore" -- /bin/bash -c "$cmd1" 27 | sleep 1 28 | gnome-terminal --tab -t "ros1_bridge" -- /bin/bash -c "$cmd2" 29 | gnome-terminal --tab -t "bag" -- /bin/bash -c "$cmd3" 30 | gnome-terminal --tab -t "TestCase_launch" -- /bin/bash -c "$cmd4" 31 | 32 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_bag3.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ukf_localization_node_bag3_ukf: 3 | ros__parameters: 4 | use_sim_time: true 5 | clear_params: true 6 | filter_type: "ukf" 7 | frequency: 30.0 8 | sensor_timeout: 0.1 9 | two_d_mode: false 10 | odom0: /ardrone/odometry/raw 11 | imu0: /ardrone/imu 12 | map_frame: map 13 | odom_frame: odom 14 | base_link_frame: base_link 15 | world_frame: odom 16 | odom0_config: [false, false, true, 17 | false, false, false, 18 | true, true, false, 19 | false, false, true, 20 | false, false, false] 21 | odom0_relative: false 22 | 23 | imu0_config: [false, false, false, 24 | true, true, true, 25 | false, false, false, 26 | true, true, true, 27 | false, false, false] 28 | 29 | # 30 | odom0_differential: false 31 | imu0_differential: false 32 | 33 | odom0_queue_size: 10 34 | imu0_queue_size: 10 35 | imu0_remove_gravitational_acceleration: true 36 | 37 | alpha: 0.001 38 | kappa: 0.0 39 | beta: 2.0 40 | 41 | process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 42 | 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 43 | 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 44 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 45 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 46 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 48 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 49 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 51 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 52 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 53 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 54 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 55 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] 56 | 57 | # 58 | test_ukf_localization_node_bag3_pose: 59 | ros__parameters: 60 | clear_params: true 61 | final_x: -0.41148 62 | final_y: -0.2513 63 | final_z: 2.990 64 | tolerance: 0.1681 65 | output_final_position: true 66 | output_location: ukf3.txt 67 | 68 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_interfaces.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2018 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | """Launch file for test_ukf_localization_node_interfaces.""" 17 | 18 | import launch 19 | from launch import LaunchDescription 20 | import launch_ros.actions 21 | import os 22 | import yaml 23 | import sys 24 | from launch.substitutions import EnvironmentVariable 25 | import pathlib 26 | import launch.actions 27 | from launch.actions import DeclareLaunchArgument 28 | from launch_testing.legacy import LaunchTestService 29 | from launch.actions import ExecuteProcess 30 | from launch import LaunchService 31 | from ament_index_python.packages import get_package_prefix 32 | 33 | def generate_launch_description(): 34 | parameters_file_dir = pathlib.Path(__file__).resolve().parent 35 | parameters_file_path = parameters_file_dir / 'test_ukf_localization_node_interfaces.yaml' 36 | os.environ['FILE_PATH'] = str(parameters_file_dir) 37 | 38 | #*****test_ukf_localization_node_interfaces.test***** 39 | ukf_node = launch_ros.actions.Node( 40 | package='robot_localization', 41 | executable='ukf_node', 42 | name='test_ukf_localization_node_interfaces_ukf', 43 | output='screen', 44 | parameters=[ 45 | parameters_file_path, 46 | str(parameters_file_path), 47 | [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_interfaces.yaml'], 48 | ],) 49 | 50 | return LaunchDescription([ 51 | ukf_node, 52 | ]) 53 | 54 | 55 | def main(argv=sys.argv[1:]): 56 | ld = generate_launch_description() 57 | 58 | test1_action = ExecuteProcess( 59 | cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_ukf_localization_node_interfaces'], 60 | output='screen', 61 | ) 62 | 63 | lts = LaunchTestService() 64 | lts.add_test_action(ld, test1_action) 65 | ls = LaunchService(argv=argv) 66 | ls.include_launch_description(ld) 67 | return lts.run(ls) 68 | 69 | if __name__ == '__main__': 70 | sys.exit(main()) 71 | -------------------------------------------------------------------------------- /test/test_ukf_localization_node_interfaces.yaml: -------------------------------------------------------------------------------- 1 | # 2 | test_ukf_localization_node_interfaces_ukf: 3 | ros__parameters: 4 | frequency: 30.0 5 | 6 | sensor_timeout: 1000. 7 | 8 | odom0: /odom_input0 9 | odom1: /odom_input1 10 | odom2: /odom_input2 11 | 12 | pose0: /pose_input0 13 | pose1: /pose_input1 14 | 15 | twist0: /twist_input0 16 | 17 | imu0: /imu_input0 18 | imu1: /imu_input1 19 | imu2: /imu_input2 20 | imu3: /imu_input3 21 | 22 | odom0_config: [true, false, true, 23 | false, false, false, 24 | false, false, false, 25 | false, false, false, 26 | false, false, false] 27 | odom0_relative: false 28 | 29 | odom1_config: [true, true, true, 30 | true, true, true, 31 | false, false, false, 32 | false, false, false, 33 | false, false, false] 34 | odom1_relative: false 35 | 36 | odom2_config: [false, false, false, 37 | false, false, false, 38 | true, true, true, 39 | true, true, true, 40 | false, false, false] 41 | odom2_relative: false 42 | 43 | pose0_config: [true, false, true, 44 | false, false, false, 45 | false, false, false, 46 | false, false, false, 47 | false, false, false] 48 | pose1_config: [true, true, true, 49 | true, true, true, 50 | false, false, false, 51 | false, false, false, 52 | false, false, false] 53 | 54 | twist0_config: [false, false, false, 55 | false, false, false, 56 | true, true, true, 57 | true, true, true, 58 | false, false, false] 59 | 60 | imu0_config: [false, false, false, 61 | true, true, true, 62 | false, false, false, 63 | false, false, false, 64 | false, false, false] 65 | imu1_config: [false, false, false, 66 | false, false, false, 67 | false, false, false, 68 | true, true, true, 69 | false, false, false] 70 | imu2_config: [false, false, false, 71 | false, false, false, 72 | false, false, false, 73 | false, false, false, 74 | true, true, true] 75 | imu3_config: [false, false, false, 76 | true, true, true, 77 | false, false, false, 78 | false, false, false, 79 | false, false, false] 80 | 81 | ### 82 | odom1_differential: true 83 | pose1_differential: true 84 | imu3_differential: true 85 | 86 | print_diagnostics: false 87 | 88 | odom_frame: odom 89 | base_link_frame: base_link 90 | 91 | process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 92 | 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 93 | 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 94 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 95 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 96 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 97 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 98 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, 101 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 102 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, 103 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 104 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 105 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] 106 | 107 | 108 | alpha: 0.001 109 | kappa: 0.0 110 | beta: 2.0 111 | --------------------------------------------------------------------------------