diff --git a/.clang-format b/.clang-format index 781851fad..346d8d832 100644 --- a/.clang-format +++ b/.clang-format @@ -4,4 +4,7 @@ Language: Cpp PointerAlignment: Left ColumnLimit: 120 AlignAfterOpenBracket: AlwaysBreak +--- +BasedOnStyle: Chromium +Language: JavaScript ... diff --git a/.github/workflows/ci_pipeline.yml b/.github/workflows/ci_pipeline.yml index 0784b5045..476de1c3b 100644 --- a/.github/workflows/ci_pipeline.yml +++ b/.github/workflows/ci_pipeline.yml @@ -89,13 +89,9 @@ jobs: working-directory: ${{ github.workspace }} run: ./src/beluga/tools/build-and-test.sh - - name: Upload documentation artifacts - uses: actions/upload-artifact@v3 - with: - name: docs - path: ./build/beluga/docs/html/ - retention-days: 7 - if: ${{ matrix.upload_artifacts }} + - name: Perform static analysis + working-directory: ${{ github.workspace }} + run: ./src/beluga/tools/run-clang-tidy.sh - name: Upload code coverage report uses: codecov/codecov-action@v3 @@ -107,14 +103,43 @@ jobs: verbose: true if: ${{ matrix.upload_artifacts }} - - name: Static analysis - working-directory: ${{ github.workspace }} - run: ./src/beluga/tools/run-clang-tidy.sh + build-docs: + needs: build-test + runs-on: ubuntu-22.04 + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + - name: Setup python + uses: actions/setup-python@v4 + with: + cache: pip + + - name: Install tooling dependencies + run: | + sudo apt-get update + sudo apt-get install -y \ + doxygen \ + git \ + graphviz \ + make \ + texlive-latex-base + pip install -r docs/requirements.txt + + - name: Build documentation + run: make -C docs html + + - name: Upload documentation artifacts + uses: actions/upload-artifact@v3 + with: + name: docs + path: ./docs/_build/html/ + retention-days: 7 deploy-docs: - if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/main' }} - needs: build-test + needs: build-docs runs-on: ubuntu-22.04 + if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/main' }} steps: - name: Checkout documentation branch uses: actions/checkout@v3 diff --git a/.gitignore b/.gitignore index b64e8758c..ddf7a251a 100644 --- a/.gitignore +++ b/.gitignore @@ -4,10 +4,14 @@ install/ lcov/ logs/ - # Python related __pycache__/ +**/venv/ # Development environments .vscode/ .idea/ + +# Sphinx / Doxygen related +_build +generated diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 43c429e5a..c61ebfa36 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -21,6 +21,7 @@ repos: - id: check-executables-have-shebangs - id: check-shebang-scripts-are-executable - id: check-yaml + args: [--allow-multiple-documents] - id: check-xml - id: end-of-file-fixer - id: trailing-whitespace diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 9379798d1..c3009b7ab 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -107,18 +107,22 @@ This projects adopts a [feature branch workflow](https://about.gitlab.com/topics #### How Do I Submit a Good Code Contribution? -1. **Fork [the Beluga repository](https://github.com/Ekumen-OS/beluga/) to your GitHub account**. -1. **Clone the repository fork locally**. +1. **Fork the [Beluga repository](https://github.com/Ekumen-OS/beluga/) to your GitHub account**. +1. **Clone the repository fork locally**. You will need `git`. ```bash git clone --recursive git@github.com:/beluga.git + ``` +1. **Install pre-commit hooks**. You will need [`pre-commit`](https://pre-commit.com). + ```bash cd beluga + pre-commit install ``` 1. **Create a new branch where your work will go**. ```bash git checkout -b /fix-issue-12345 main ``` 1. **Work on your contribution**. See [instructions](GETTING_STARTED.md) on how to get started with Beluga development. -1. **Lint and test your changes**. For bug fixes, make sure regression tests are included. +1. **Test your changes**. For bug fixes, make sure regression tests are included. 1. **Document your changes as needed**. For new features, make sure added functionality is clearly documented. 1. **Push the branch to your fork on GitHub**. 1. **Open a pull request**. Make sure all tests and linters pass on your branch before opening. diff --git a/DEVELOPING.md b/DEVELOPING.md new file mode 100644 index 000000000..592d85a42 --- /dev/null +++ b/DEVELOPING.md @@ -0,0 +1,76 @@ +# Developing Beluga + +## Table of Contents + +- [Environment](#environment) +- [Workflow](#workflow) +- [CI/CD](#ci-cd) + +## Environment + +Beluga developers use [Docker](https://www.docker.com) containers as development environment. These containers include both project dependencies and development tooling for each of the supported platforms. Source code [bind mounts](https://docs.docker.com/storage/bind-mounts) afford Beluga developers the freedom of applying their own personal programming workflows to Beluga (e.g. which editor to use), so long as they are compatible with the project guidelines. + +To bring up a development environment: + +1. **Clone the repository**. You will need `git`. + + ```bash + git clone --recursive git@github.com:Ekumen-OS/beluga.git + ``` + +2. **Build and run the development container**. You will need [`docker-compose v2.10+`](https://github.com/docker/compose/tree/v2). + + ```bash + (cd beluga && docker/run.sh) + ``` + + To rebuild the image before starting the container, use: + + ```bash + (cd beluga && docker/run.sh --build) + ``` + + To target an specific ROS distribution, use: + + ```bash + (cd beluga && ROSDISTRO=humble docker/run.sh) + ``` + + Supported distributions include `noetic`, `humble`, `iron`, and `rolling`. + +## Workflow + +The Beluga project started as a ROS 2 project and therefore relies on a typical ROS 2 workflow. In particular, the Beluga project subscribes to [REP-2004](https://ros.org/reps/rep-2004.html) and aims for Quality Level 1. For development, this translates to mandatory API documentation and a 95% code coverage policy for unit and integration testing. + +Within a development environment: + +1. **Build and test the project**. You will usually use `colcon`. + + ```bash + cd /ws + colcon build --symlink-install + colcon test + ``` + + You may also use `catkin_make_isolated` and `catkin-tools` for ROS 1 distributions. + +2. **Update package dependencies**. Listed in `package.xml` files. + + ```bash + cd /ws + rosdep update + rosdep install --from-path src + ``` + +For more advanced tooling, check repository [tools](./tools). + +## CI/CD + +Every pull request and every push to the project repository `main` branch will be subject to a [continuous integration workflow](./.github/workflows/ci_pipeline.yml). This workflow will lint project sources, static analyze, build, and test project packages against the project platform support matrix, enforcing its code coverage policy, and build project documentation (which will be deployed to Github Pages when pushing to `main`). + +Pull request acceptance is predicated on these checks passing. + +## Next Steps + +- Go over the [project documentation](https://ekumen-os.github.io/beluga) to better understand what, how, and why. +- If you want to contribute to this project, please read the [contribuing guidelines](CONTRIBUTING.md) first. diff --git a/GETTING_STARTED.md b/GETTING_STARTED.md deleted file mode 100644 index e6762428e..000000000 --- a/GETTING_STARTED.md +++ /dev/null @@ -1,84 +0,0 @@ -# Getting started with Beluga - -## Crash course - -1. **Clone the repository**. You will need `git`. - - ```bash - git clone --recursive git@github.com:Ekumen-OS/beluga.git - ``` - -1. **Build and run the development docker container**. You will need [`docker-compose v2.10+`](https://github.com/docker/compose/tree/v2). - - ```bash - (cd beluga && docker/run.sh) - ``` - To rebuild the image before starting the container, use: - ```bash - (cd beluga && docker/run.sh --build) - ``` - To target an specific ROS distribution, use: - ```bash - (cd beluga && ROSDISTRO=humble docker/run.sh) - ``` - Supported distributions include `noetic`, `humble`, `iron`, and `rolling`. - -1. **Build and test the project** (inside development container). - - ```bash - cd /ws - colcon build --symlink-install - colcon test - ``` - - You may also use `catkin_make_isolated` and `catkin-tools` in ROS 1 distributions. - -1. **Run an example application using a ROS bag** (inside development container). - - For ROS 2 distributions, run: - ```bash - cd /ws - source install/setup.bash - ros2 launch beluga_example perfect_odometry.launch.xml - ``` - - For ROS 1 distributions, run: - ```bash - cd /ws - source devel*/setup.bash - roslaunch beluga_example perfect_odometry.launch - ``` - -1. **Run an example application using a simulation and teleop controls** (inside development container). - - For ROS 2 distributions, in two separate terminals run: - ```bash - cd /ws - source install/setup.bash - ros2 launch beluga_example simulation.launch.xml - ``` - ```bash - ros2 run teleop_twist_keyboard teleop_twist_keyboard - ``` - - For ROS 1 distributions, in two separate terminals run: - ```bash - cd /ws - source devel*/setup.bash - roslaunch beluga_example simulation.launch - ``` - ```bash - rosrun teleop_twist_keyboard teleop_twist_keyboard - ``` - -1. **Lint your code**. You will need [`pre-commit`](https://pre-commit.com/). - - ```bash - pre-commit run --all-files - ``` - -## Next steps - -- Check the [installation guide](INSTALLING.md) if you want to test Beluga AMCL on a robot. -- If you want to contribute to this project, please read the [contribuing guidelines](CONTRIBUTING.md). -- For more advanced tools useful for contributing, check out the [tools](./tools/) directory. diff --git a/INSTALLING.md b/INSTALLING.md deleted file mode 100644 index 872183d4e..000000000 --- a/INSTALLING.md +++ /dev/null @@ -1,78 +0,0 @@ -# Installing Beluga AMCL on a Robot - -## Installing From Source - -1. **Create a workspace directory**. - - ```bash - mkdir -p ~/ws/src - ``` - -1. **Clone the repository into your workspace**. - - ```bash - cd ~/ws - git clone https://github.com/Ekumen-OS/beluga.git ~/ws/src/beluga - ``` - -1. **Source the ROS installation**. - - ```bash - source /opt/ros/${ROS_DISTRO}/setup.bash - ``` - -1. **Install dependencies**. - - ```bash - rosdep install -r --from-paths ~/ws/src/beluga -y -i -t build -t exec --skip-keys 'flatland_server flatland_plugins' - ``` - -1. **Build and source the workspace**. - - In ROS 2 and ROS 1 distributions (if [`colcon`](https://colcon.readthedocs.io/en/released/user/installation.html) is installed), run: - ```bash - colcon build --packages-up-to beluga_example --cmake-args -DBUILD_TESTING=OFF -DBUILD_DOCS=OFF - source install/setup.bash - ``` - - In ROS 1 distributions, you may run the following command instead: - ```bash - catkin_make_isolated --only-pkg-with-deps beluga_example --install --cmake-args -DBUILD_TESTING=OFF -DBUILD_DOCS=OFF - source devel*/setup.bash - ``` - -1. **Launch a localization node manually**. - - For ROS 2 distributions, run: - ```bash - ros2 launch beluga_example localization_launch.py use_composition:=True localization_params_file:= localization_map:= - ``` - - For ROS 1 distributions, run: - ```bash - roslaunch beluga_example localization.launch localization_params_file:= localization_map:= - ``` - - The `localization_params_file` argument can be ommited if the [default AMCL parameters](beluga_example/params/default.ros2.yaml) are compatible with the robot. - -1. **Use RViz to visualize the localization output**. - - For ROS 2 distributions, run: - ```bash - rviz2 -d $(ros2 pkg prefix --share beluga_example)/rviz/rviz.ros2.rviz - ``` - - For ROS 1 distributions, run: - ```bash - rviz -d $(rospack find beluga_example)/rviz/rviz.ros.rviz - ``` - - **Quality of Service** - - In ROS 2, when subscribing to the output topics from localization, we recommend the following [QoS](https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Quality-of-Service-Settings.html) settings: - - | Topic | Depth | History | Reliability | Durability | - |------------------|-------|--------------|--------------|-----------------| - | `map` | 5 | Keep last | Reliable | Transient local | - | `particle_cloud` | 5 | Keep last | Best effort | Volatile | - | `pose` | 5 | Keep last | Reliable | Volatile | diff --git a/README.md b/README.md index 595e91648..b498a940c 100644 --- a/README.md +++ b/README.md @@ -42,7 +42,5 @@ This repository contains the following packages: ## ⚙️ First Steps -- Get hands-on experience with the [getting started](GETTING_STARTED.md) tutorial. -- Check the [installation guide](INSTALLING.md) to test this on a robot. -- Check the [API documentation](https://ekumen-os.github.io/beluga/) for information on how to use the library in your project. +- Go check the [project documentation](https://ekumen-os.github.io/beluga). - Read the [contributing guidelines](CONTRIBUTING.md). diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index ba1217061..1a7b46755 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -75,12 +75,6 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -option(BUILD_DOCS "Build API documentation." OFF) -if(BUILD_DOCS) - message(STATUS "Build API documentation enabled.") - add_subdirectory(docs) -endif() - set(INSTALL_CMAKEDIR ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cmake) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) diff --git a/beluga/docs/CMakeLists.txt b/beluga/docs/CMakeLists.txt deleted file mode 100644 index 6dc73e026..000000000 --- a/beluga/docs/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright 2023 Ekumen, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -find_package(Doxygen) - -if(DOXYGEN_FOUND) - include(FetchContent) - - FetchContent_Declare( - doxygen-awesome-css - GIT_REPOSITORY https://github.com/jothepro/doxygen-awesome-css - GIT_TAG v2.2.0 - SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/doxygen-awesome) - - FetchContent_MakeAvailable(doxygen-awesome-css) - - # cmake-lint: disable=C0301 - # Line too long - add_custom_target( - beluga_docs ALL - COMMAND - DOXYGEN_OUTPUT_DIRECTORY=${CMAKE_CURRENT_BINARY_DIR} - DOXYGEN_HTML_STYLESHEET=${CMAKE_CURRENT_BINARY_DIR}/doxygen-awesome/doxygen-awesome.css - ${DOXYGEN_EXECUTABLE} ./Doxyfile - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - COMMENT "Generating API documentation with Doxygen" - VERBATIM) -else() - message(WARNING "Documentation skipped, doxygen not found.") -endif() diff --git a/beluga/docs/Doxyfile b/beluga/docs/Doxyfile index 9c74f26c3..5e1788632 100644 --- a/beluga/docs/Doxyfile +++ b/beluga/docs/Doxyfile @@ -1,13 +1,14 @@ PROJECT_NAME = "Beluga" -PROJECT_BRIEF = "An extensible particle filter library." -OUTPUT_DIRECTORY = $(DOXYGEN_OUTPUT_DIRECTORY) -INPUT = ../include ./README.md +INPUT = ../include ./_doxygen/beluga-main.md EXAMPLE_PATH = ../test/beluga RECURSIVE = YES -USE_MDFILE_AS_MAINPAGE = ./README.md +USE_MDFILE_AS_MAINPAGE = ./_doxygen/beluga-main.md GENERATE_HTML = YES GENERATE_LATEX = NO -HTML_EXTRA_STYLESHEET = $(DOXYGEN_HTML_STYLESHEET) -CITE_BIB_FILES = ./doxygen_cites.bib +CITE_BIB_FILES = ./references.bib WARN_AS_ERROR = YES -PROJECT_LOGO = ./img/logo_200x200.png +SHOW_NAMESPACES = YES +USE_MATHJAX = YES +HAVE_DOT = YES +DOT_IMAGE_FORMAT = svg +DOT_TRANSPARENT = YES diff --git a/beluga/docs/Makefile b/beluga/docs/Makefile new file mode 100644 index 000000000..a62ead8e9 --- /dev/null +++ b/beluga/docs/Makefile @@ -0,0 +1,31 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +FIND ?= find +RM ?= rm + +html: Makefile + @$(SPHINXBUILD) -M html "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +clean: + @$(RM) -fr "$(BUILDDIR)" `$(FIND) "$(SOURCEDIR)" -name generated -type d` + +.PHONY: clean Makefile diff --git a/beluga/docs/README.md b/beluga/docs/_doxygen/beluga-main.md similarity index 86% rename from beluga/docs/README.md rename to beluga/docs/_doxygen/beluga-main.md index 8b6f0c764..fb5de4c6f 100644 --- a/beluga/docs/README.md +++ b/beluga/docs/_doxygen/beluga-main.md @@ -1,22 +1,15 @@ -# Beluga +# API Reference -[TOC] +This is the API reference for Beluga. It extends concepts from the C++ standard library (STL), emphasizing support for ranges of particles. -## Introduction - -Beluga is a ROS-agnostic C++17 library that provides implementations for Monte Carlo-based localization algorithms widely used in robotics applications. -Its modularity allows users to compose solutions from reusable modules and to combine them with new ones to configure the MCL algorithm that best suits their needs. - -The API extends concepts from the STL, emphasizing support for ranges of particles. - -## Components +### Components Explore the library's components organized by concept: -### Sensor models +#### Sensor models Probabilistic sensor models that describe the likelihood of obtaining a certain measurement given a specific state of the system. -They satisfy the [sensor model named requirements](@ref SensorModelPage). +They all satisfy the [sensor model named requirements](@ref SensorModelPage). | | | |-|-| @@ -25,7 +18,7 @@ They satisfy the [sensor model named requirements](@ref SensorModelPage). | beluga::LandmarkSensorModel | Generic landmark model for discrete detection sensors (both 2D and 3D) | | beluga::BearingSensorModel | Generic bearing sensor model for discrete detection sensors (both 2D and 3D) | -### Motion models +#### Motion models Probabilistic motion models that describe how the state of the system evolves over time. They satisfy the [motion model named requirements](@ref MotionModelPage). @@ -35,7 +28,7 @@ They satisfy the [motion model named requirements](@ref MotionModelPage). | beluga::DifferentialDriveModel | Sampled odometry model for a differential drive | | beluga::OmnidirectionalDriveModel | Sampled odometry model for an omnidirectional drive | -### Range views +#### Range views Useful views for working with ranges of particles. They are lazily evaluated range adaptor objects compatible with the Range-v3 library. @@ -51,7 +44,7 @@ They are lazily evaluated range adaptor objects compatible with the Range-v3 lib | [beluga::views::weights](@ref views/particles.hpp) | Produces a view of the weights of a range of particles | | [beluga::views::zip](@ref views/zip.hpp) | Given N ranges, return a new range where the Mth element is a tuple of the Mth elements of all N ranges | -### Range actions +#### Range actions Useful actions for working with ranges of particles. They are eagerly evaluated range adaptor objects compatible with the Range-v3 library. @@ -63,7 +56,7 @@ They are eagerly evaluated range adaptor objects compatible with the Range-v3 li | [beluga::actions::propagate](@ref actions/propagate.hpp) | Updates particle states based on their current value and a state transition (or sampling) function | | [beluga::actions::reweight](@ref actions/reweight.hpp) | Updates particle weights based on a given measurement likelihood function | -### Policies +#### Policies Policies that can be used to decide when to update the particle filter or resample the current set of particles. They are lazily-evaluated possibly stateful predicate that can be composed with others using overloaded boolean operators. @@ -74,14 +67,14 @@ They are lazily-evaluated possibly stateful predicate that can be composed with | [beluga::policies::on_effective_size_drop](@ref policies/on_effective_size_drop.hpp) | Triggers when the Effective Sample Size (ESS) drops below a certain threshold | | [beluga::policies::on_motion](@ref policies/on_motion.hpp) | Triggers on the detected motion | -### Containers +#### Containers | | | |-|-| | beluga::CircularArray | An implementation of generic circular array modeled after `std::array` | | beluga::TupleVector | An implementation of a tuple of containers with an interface that looks like a container of tuples | -### Distributions +#### Distributions | | | |-|-| diff --git a/beluga/docs/_images b/beluga/docs/_images new file mode 120000 index 000000000..097050528 --- /dev/null +++ b/beluga/docs/_images @@ -0,0 +1 @@ +../../docs/_images \ No newline at end of file diff --git a/beluga/docs/_static b/beluga/docs/_static new file mode 120000 index 000000000..7fdcaa74f --- /dev/null +++ b/beluga/docs/_static @@ -0,0 +1 @@ +../../docs/_static \ No newline at end of file diff --git a/beluga/docs/conf.py b/beluga/docs/conf.py new file mode 100644 index 000000000..b597125e3 --- /dev/null +++ b/beluga/docs/conf.py @@ -0,0 +1,85 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Configuration file for the Sphinx documentation builder. +# +# For the full list of built-in configuration values, see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +import subprocess + +# -- Project information ----------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information + +project = "Beluga" +copyright = "2022-2024 Ekumen, Inc." +author = "Ekumen Research" + +# -- General configuration --------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration +source_suffix = { + ".rst": "restructuredtext", + ".md": "markdown", +} +language = "en" + +extensions = [] +extensions = [ + "myst_parser", + "sphinx.ext.mathjax", + "sphinx.ext.duration", + "sphinx.ext.graphviz", + "sphinx_copybutton", + "sphinx.ext.inheritance_diagram", + "sphinx_design", + "sphinx_babel.autodox", + "sphinxcontrib.bibtex", +] + +bibtex_bibfiles = ["references.bib"] + +myst_enable_extensions = ["colon_fence"] +myst_heading_anchors = 4 + +autodox_outdir = "_doxygen/generated" +autodox_projects = {"reference": ""} + +exclude_patterns = ["README.md", "_build", "_doxygen/*.md"] +suppress_warnings = ["myst.header"] + +# -- Options for HTML output ------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output + +html_theme = "sphinx_book_theme" +html_theme_options = { + "show_navbar_depth": 1, + "collapse_navbar": True, + "collapse_navigation": True, + "repository_branch": "main", + "repository_url": "https://github.com/Ekumen-OS/beluga", + "use_repository_button": True, + "use_edit_page_button": False, + "home_page_in_toc": True, + "logo": { + "image_light": "_images/logo_with_name_light.png", + "image_dark": "_images/logo_with_name_dark.png", + }, +} +html_last_updated_fmt = subprocess.check_output( + ["git", "log", "--pretty=format:'%ad, %h'", "-n1"] +).decode("utf-8") +html_favicon = "_images/logo_200x200.png" +html_css_files = ["custom.css"] +html_js_files = ["custom.js"] +html_static_path = ["_static"] diff --git a/beluga/docs/index.md b/beluga/docs/index.md new file mode 100644 index 000000000..53f74b1c2 --- /dev/null +++ b/beluga/docs/index.md @@ -0,0 +1,46 @@ +# Overview + +```{toctree} +:hidden: +:maxdepth: 1 + +API Reference <_doxygen/generated/reference/html/index> +``` + +Beluga is a C++17 library that provides Monte Carlo Localization (MCL) algorithms' implementations widely used in robotics applications. +Its modularity allows users to compose solutions from reusable modules and to combine them with new ones to configure the MCL algorithm +that best suits their needs. + +## Features + +The current set of features includes: + +- Particle containers: + - Support for [Array-of-Structures and Structure-of-Arrays][aos_soa] tuple containers +- Composable range adaptor views, actions, and algorithms: + - Multivariate normal distributions in SE(2) and SE(3) space + - Multivariate uniform distributions in SE(2) compatible with occupancy grids + - Multinomial resampling from a particle range + - [Adaptive KLD resampling][fox2001] + - [Selective resampling][grisetti2007], on-motion resampling, and interval resampling policies + - Support for sequential and parallel execution policies + - Weighted mean and covariance statistics for pose estimation +- Sensor models: + - Likelihood field model + - Beam model + - Landmark-based models (using landmark position or bearing) +- Motion models: + - Differential drive model + - Omnidirectional model + +## Dependencies + +Beluga is built on top of the following open source libraries: + +- [Eigen](https://gitlab.com/libeigen/eigen): A well-known C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms. +- [Sophus](https://github.com/strasdat/Sophus): A C++ implementation of Lie groups using Eigen. +- [Range](https://github.com/ericniebler/range-v3): The basis library for C++20's `std::ranges`. + +[aos_soa]: https://en.wikipedia.org/wiki/AoS_and_SoA +[fox2001]: https://dl.acm.org/doi/10.5555/2980539.2980632 +[grisetti2007]: https://doi.org/10.1109/TRO.2006.889486 diff --git a/beluga/docs/doxygen_cites.bib b/beluga/docs/references.bib similarity index 100% rename from beluga/docs/doxygen_cites.bib rename to beluga/docs/references.bib diff --git a/beluga/include/beluga/algorithm/distance_map.hpp b/beluga/include/beluga/algorithm/distance_map.hpp index 45ec6858a..bf5f08649 100644 --- a/beluga/include/beluga/algorithm/distance_map.hpp +++ b/beluga/include/beluga/algorithm/distance_map.hpp @@ -27,6 +27,8 @@ * \brief Implementation of algorithm to calculate distance from obstacles. */ +namespace beluga { + /// Returns a map where the value of each cell is the distance to the nearest obstacle. /** * The algorithm uses O(N) time and memory, where `N=ranges::size(obstacle_map)`. @@ -90,4 +92,6 @@ auto nearest_obstacle_distance_map( return distance_map; } +} // namespace beluga + #endif diff --git a/beluga/include/beluga/algorithm/estimation.hpp b/beluga/include/beluga/algorithm/estimation.hpp index c80d2ec42..5397d5397 100644 --- a/beluga/include/beluga/algorithm/estimation.hpp +++ b/beluga/include/beluga/algorithm/estimation.hpp @@ -169,13 +169,16 @@ std::pair, Sophus::Matrix3> estimate(Poses&& poses, return std::pair{estimated_pose, covariance_matrix}; } -/// Returns a pair consisting of the estimated mean pose and its covariance. +/// Computes mean and covariance Returns a pair consisting of the estimated mean pose and its covariance. /** - * Given a range of poses, computes the estimated pose by averaging the translation + * Given a range of 2D poses, computes the estimated pose by averaging the translation * and rotation parts, assuming all poses are equally weighted. * Computes the covariance matrix of the translation parts and the circular variance * of the rotation angles to create a 3x3 covariance matrix. - * It does not take into account the particle weights. + * It does not take into account the particle weights. This is appropriate for use with + * filter update cycles that resample the particle set at every iteration, since + * in that case the belief is fully represented by the spatial distribution of the + * particles, and the particle weights provide no additional information. * * \tparam Poses A [sized range](https://en.cppreference.com/w/cpp/ranges/sized_range) type whose * value type is `Sophus::SE2`. diff --git a/beluga/include/beluga/beluga.hpp b/beluga/include/beluga/beluga.hpp index fb3efb095..f05385212 100644 --- a/beluga/include/beluga/beluga.hpp +++ b/beluga/include/beluga/beluga.hpp @@ -20,6 +20,11 @@ * \brief Includes all the Beluga API. */ +/** + * \namespace beluga + * \brief The main Beluga namespace + */ + #include #include #include diff --git a/beluga/include/beluga/sensor/beam_model.hpp b/beluga/include/beluga/sensor/beam_model.hpp index 855269718..2171873f3 100644 --- a/beluga/include/beluga/sensor/beam_model.hpp +++ b/beluga/include/beluga/sensor/beam_model.hpp @@ -58,9 +58,15 @@ struct BeamModelParam { /// Beam sensor model for range finders. /** - * This class satisfies \ref SensorModelPage. + * This model closely matches the physical behavior of a lidar, but it is + * computationally expensive because a ray-tracing operation needs to be + * performed for each particle and individual range reading. Also, this + * model can experience degraded performance in cluttered environments + * due the inherent lack of smoothness of the sensor readings in such + * settings. See Probabilistic Robotics \cite thrun2005probabilistic + * Chapter 6.2 for further reference. * - * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.2. + * \note This class satisfies \ref SensorModelPage. * * \tparam OccupancyGrid Type representing an occupancy grid. * It must satisfy \ref OccupancyGrid2Page. diff --git a/beluga/include/beluga/sensor/bearing_sensor_model.hpp b/beluga/include/beluga/sensor/bearing_sensor_model.hpp index 4cd43f976..8c525b610 100644 --- a/beluga/include/beluga/sensor/bearing_sensor_model.hpp +++ b/beluga/include/beluga/sensor/bearing_sensor_model.hpp @@ -46,7 +46,13 @@ struct BearingModelParam { /// Generic bearing sensor model, for both 2D and 3D state types. /** - * This class satisfies \ref SensorModelPage. + * The model only relies on the bearing of a landmark detection relative to the + * sensor when calculating the updated importance weight. This can be used to + * support bearing-only measurements, such as feature detections in monocular + * images. + * + * \note This class satisfies \ref SensorModelPage. + * * \tparam LandmarkMap class managing the list of known landmarks. * \tparam StateType type of the state of the particle. */ diff --git a/beluga/include/beluga/sensor/landmark_sensor_model.hpp b/beluga/include/beluga/sensor/landmark_sensor_model.hpp index f1d3d857d..e38e1ec5f 100644 --- a/beluga/include/beluga/sensor/landmark_sensor_model.hpp +++ b/beluga/include/beluga/sensor/landmark_sensor_model.hpp @@ -48,10 +48,13 @@ struct LandmarkModelParam { /// Generic landmark model for discrete detection sensors (both 2D and 3D). /** - * This class satisfies \ref SensorModelPage. + * This sensor model is a generalization of the Sensor Model with Known Correspondences + * described in Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.6. This version + * has been extended to support landmark detection in 3D even when the robot is constrained to + * 2D motion. The importance weight function models detections as having two components that + * can be independently measured: range and bearing. * - * This sensor model is a generalization of the model described in Probabilistic - * Robotics \cite thrun2005probabilistic Chapter 6.6 . + * \note This class satisfies \ref SensorModelPage. * * \tparam LandmarkMap class managing the list of known landmarks. * \tparam StateType type of the state of the particle. diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp index a505e0250..cbff7f617 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model.hpp @@ -62,9 +62,14 @@ struct LikelihoodFieldModelParam { /// Likelihood field sensor model for range finders. /** - * This class satisfies \ref SensorModelPage. + * This model relies on a pre-computed likelihood map of the environment. + * It is less computationally intensive than the beluga::BeamSensorModel + * because no ray-tracing is required, and it can also provide better + * performance in environments with non-smooth occupation maps. See + * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4, + * for further reference. * - * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4. + * \note This class satisfies \ref SensorModelPage. * * \tparam OccupancyGrid Type representing an occupancy grid. * It must satisfy \ref OccupancyGrid2Page. diff --git a/beluga/test/beluga/algorithm/test_distance_map.cpp b/beluga/test/beluga/algorithm/test_distance_map.cpp index 60ade584d..e35d85593 100644 --- a/beluga/test/beluga/algorithm/test_distance_map.cpp +++ b/beluga/test/beluga/algorithm/test_distance_map.cpp @@ -37,37 +37,37 @@ auto make_neighbors_function(std::size_t size) { TEST(DistanceMap, None) { auto map = std::vector{}; - auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); ASSERT_EQ(distance_map.size(), 0); } TEST(DistanceMap, Empty) { auto map = std::array{false, false, false, false, false, false}; - auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0)); } TEST(DistanceMap, Full) { auto map = std::array{true, true, true, true, true, true}; - auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0)); } TEST(DistanceMap, Case1) { auto map = std::array{false, true, false, false, false, true}; - auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); ASSERT_THAT(distance_map, testing::ElementsAre(1, 0, 1, 2, 1, 0)); } TEST(DistanceMap, Case2) { auto map = std::array{true, true, false, false, false, false}; - auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 1, 2, 3, 4)); } TEST(DistanceMap, Case3) { auto map = std::array{false, false, false, false, false, true}; - auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + auto distance_map = beluga::nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); ASSERT_THAT(distance_map, testing::ElementsAre(5, 4, 3, 2, 1, 0)); } diff --git a/beluga_amcl/README.md b/beluga_amcl/README.md index 72a1352a5..cce86d12b 100644 --- a/beluga_amcl/README.md +++ b/beluga_amcl/README.md @@ -28,7 +28,7 @@ The compatibility between `beluga_amcl` and its longstanding counterparts in the ### Parameters Beluga AMCL currently supports the majority of ROS parameters used in [Navigation 2 AMCL][nav2_amcl].
-See [Beluga AMCL parameter reference](docs/PARAMETERS.md) for detailed information. +See [Beluga AMCL documentation](https://ekumen-os.github.io/beluga/packages/beluga_amcl/docs) for further reference. ### Subscribed Topics diff --git a/beluga_amcl/docs/Doxyfile b/beluga_amcl/docs/Doxyfile new file mode 100644 index 000000000..5f72d5cd9 --- /dev/null +++ b/beluga_amcl/docs/Doxyfile @@ -0,0 +1,10 @@ +PROJECT_NAME = "Beluga AMCL" +INPUT = ../include ./_doxygen/beluga_amcl-main.md +EXAMPLE_PATH = ../test +RECURSIVE = YES +USE_MDFILE_AS_MAINPAGE = ./_doxygen/beluga_amcl-main.md +GENERATE_HTML = YES +GENERATE_LATEX = NO +WARN_AS_ERROR = YES +SHOW_NAMESPACES = YES +USE_MATHJAX = YES diff --git a/beluga_amcl/docs/Makefile b/beluga_amcl/docs/Makefile new file mode 100644 index 000000000..a62ead8e9 --- /dev/null +++ b/beluga_amcl/docs/Makefile @@ -0,0 +1,31 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +FIND ?= find +RM ?= rm + +html: Makefile + @$(SPHINXBUILD) -M html "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +clean: + @$(RM) -fr "$(BUILDDIR)" `$(FIND) "$(SOURCEDIR)" -name generated -type d` + +.PHONY: clean Makefile diff --git a/beluga_amcl/docs/PARAMETERS.md b/beluga_amcl/docs/PARAMETERS.md deleted file mode 100644 index e5126043e..000000000 --- a/beluga_amcl/docs/PARAMETERS.md +++ /dev/null @@ -1,202 +0,0 @@ -# Beluga AMCL Parameter Reference - -This page describes the parameters supported by Beluga AMCL including comparison tables with Nav2 AMCL and AMCL, along with relevant compatibility notes. - -## Table of Contents - -- [ROS 2 Reference](#ros-2-reference) - - [Interface Parameters](#interface-parameters) - - [Initial Pose and Transform Broadcast Parameters](#initial-pose-and-transform-broadcast-parameters) - - [Particle Filter Parameters](#particle-filter-parameters) - - [Motion Model Parameters](#motion-model-parameters) - - [Observation Model Parameters](#observation-model-parameters) - - [Compatibility Notes](#compatibility-notes) -- [ROS 1 Reference](#ros-1-reference) - - [Interface Parameters](#interface-parameters-1) - - [Initial Pose and Transform Broadcast Parameters](#initial-pose-and-transform-broadcast-parameters-1) - - [Particle Filter Parameters](#particle-filter-parameters-1) - - [Motion Model Parameters](#motion-model-parameters-1) - - [Observation Model Parameters](#observation-model-parameters-1) - - [Diagnostics Parameters](#diagnostics-parameters) - - [Compatibility Notes](#compatibility-notes-1) -- [Additional Notes](#additional-notes) -- [References](#references) - -## ROS 2 Reference - -### Interface Parameters - -| Parameter | Description | Navigation 2 AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `base_frame_id` | Robot base frame name rigidly attached to the mobile robot base. | ✅ | ✅ | -| `odom_frame_id` | Odometry frame name. The pose of a mobile platform relative to this frame can drift over time but it must be continuous (without discrete jumps). | ✅ | ✅ | -| `global_frame_id` | Map frame name. This node can estimate and publish the transform between global and odometry frames. Pose and map messages should use this coordinate frame. | ✅ | ✅ | -| `scan_topic` | The name of the topic where laser scans will be published. A transform must exist between the coordinate frame used in the scan messages and the base frame of the robot. | ✅ | ✅ | -| `map_topic` | The name of the topic where the map will be published by the map server. | ✅ | ✅ | -| `initial_pose_topic` | The name of the topic where an initial pose can be published.
_A parameter that allows changing the topic name doesn't exist in Nav2 AMCL, but the `initialpose` topic can be [remapped externally](https://design.ros2.org/articles/static_remapping.html)._ | | ✅ | - -### Initial Pose and Transform Broadcast Parameters - -| Parameter | Description | Navigation 2 AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `set_initial_pose` | Whether to set initial pose from the `initial_pose*` parameters or wait for an initial pose message. | ✅ | ✅ | -| `initial_pose.[x, y, yaw]` | X, Y and yaw coordinates of initial pose of robot base frame in global frame. | ✅ | ✅ | -| `initial_pose.covariance_[x, y, yaw, xy, xyaw, yyaw]` | Covariance to use with the initial pose when initializing the particle filter. _Nav2 AMCL considers these to be zero._ | | ✅ | -| `always_reset_initial_pose` | Whether to wait for an initial pose provided either via topic or `initial_pose*` parameter when reset or use the last known pose to initialize. | ✅ | ✅ | -| `first_map_only` | Whether to ignore any other map messages on the `map` topic after the first one. | ✅ | ✅ | -| `save_pose_rate` | Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server. _This parameter exists in Nav AMCL in ROS 1, but it wasn't ported to ROS 2 since there is no parameter server._ | | | -| `tf_broadcast` | Whether to publish the transform between the global frame and the odometry frame. The transform will be published only if an initial pose was set via topic or parameters, or if global localization was requested via the provided service. | ✅ | ✅ | -| `transform_tolerance` | Time by which to post-date the global to odom transform to indicate that it is valid in the future. | ✅ | ✅ | - -### Particle Filter Parameters - -| Parameter | Description | Navigation 2 AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `max_particles` | Maximum allowed number of particles. | ✅ | ✅ | -| `min_particles` | Minimum allowed number of particles. | ✅ | ✅ | -| `pf_z` | Upper standard normal quantile for $P$, where $P$ is the probability that the error in the estimated distribution will be less than `pf_err` in KLD resampling [[1]](#1). | ✅ | ✅ | -| `pf_err` | Maximum particle filter population error between the true distribution and the estimated distribution. It is used in KLD resampling [[1]](#1) to limit the allowed number of particles to the minimum necessary. | ✅ | ✅ | -| `spatial_resolution_[x, y, theta]` | Spatial resolution to create buckets for KLD resampling [[1]](#1). | | ✅ | -| `recovery_alpha_fast` | Exponential decay rate for the fast average weight filter, used in deciding when to recover from a bad approximation by adding random poses [[3]](#3). | ✅ | ✅ | -| `recovery_alpha_slow` | Exponential decay rate for the slow average weight filter, used in deciding when to recover from a bad approximation by adding random poses [[3]](#3). | ✅ | ✅ | -| `resample_interval` | Number of filter updates required before resampling. | ✅ | ✅ | -| `selective_resampling` | Whether to enable selective resampling [[2]](#2) to help avoid loss of diversity in the particle population. The resampling step will only happen if the effective number of particles $(N_{eff} = 1/ {\sum w_i^2})$ is lower than half the current number of particles, where $w_i$ refers to the normalized weight of each particle.
_This feature is currently supported by Nav AMCL in ROS 1 but it hasn't been ported to ROS 2 at the time of this writing._ | | ✅ | -| `update_min_a` | Rotational movement required from last resample for resampling to happen again. See [compatibility notes](#compatibility-notes). | ✅ | ✅ | -| `update_min_d` | Translational movement required from last resample for resampling to happen again. See [compatibility notes](#compatibility-notes). | ✅ | ✅ | -| `execution_policy` | Execution policy used to apply the motion update and importance weight steps to each particle (`seq` for sequential execution and `par` for parallel execution). | | ✅ | - -### Motion Model Parameters - -| Parameter | Description | Navigation 2 AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `robot_model_type` | Which odometry motion model to use. Supported models are `differential_drive` [[3]](#3), `omnidirectional_drive` and `stationary`. See [compatibility notes](#compatibility-notes). | ✅ | ✅ | -| `alpha1` | Expected process noise in odometry’s rotation estimate from rotation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `alpha2` | Expected process noise in odometry’s rotation estimate from translation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `alpha3` | Expected process noise in odometry’s translation estimate from translation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `alpha4` | Expected process noise in odometry’s translation estimate from rotation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `alpha5` | Expected process noise in odometry's strafe estimate from translation for the omnidirectional drive model. | ✅ | ✅ | - -### Observation Model Parameters - -| Parameter | Description | Navigation 2 AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `laser_model_type` | Which observation model to use. Supported models are `beam` and `likelihood_field` as described in [[3]](#3) with the same aggregation formula used in Nav2 AMCL. | ✅ | ✅ | -| `laser_max_range` | Maximum scan range to be considered. | ✅ | ✅ | -| `laser_min_range` | Minimum scan range to be considered. | ✅ | ✅ | -| `max_beams` | How many evenly spaced beams in each scan will be used when updating the filter. | ✅ | ✅ | -| `sigma_hit` | Standard deviation of the hit distribution used in likelihood field and beam models. | ✅ | ✅ | -| `z_hit` | Mixture weight for the probability of hitting an obstacle used in likelihood field and beam models. | ✅ | ✅ | -| `z_rand` | Mixture weight for the probability of getting random measurements used in likelihood field and beam models. | ✅ | ✅ | -| `z_max` | Mixture weight for the probability of getting max range measurements used in the beam model. | ✅ | ✅ | -| `z_short` | Mixture weight for the probability of getting short measurements used in the beam model. | ✅ | ✅ | -| `lambda_short` | Short readings' exponential distribution parameter used in the beam model. | ✅ | ✅ | -| `laser_likelihood_max_dist` | Maximum distance to do obstacle inflation on map used in the likelihood field model. | ✅ | ✅ | -| `do_beamskip` | Whether to ignore the beams for which the majority of the particles do not match the map in the likelihood field model. | ✅ | | -| `beam_skip_distance` | Maximum distance to an obstacle to consider that a beam coincides with the map. | ✅ | | -| `beam_skip_threshold` | Minimum percentage of particles for which a particular beam must match the map to not be skipped. | ✅ | | -| `beam_skip_error_threshold` | Maximum percentage of skipped beams. Too many skipped beams trigger a full update to recover in case of bad convergence. | ✅ | | - -### Compatibility Notes - -- Beluga AMCL supports Nav2 AMCL plugin names (`nav2_amcl::DifferentialMotionModel`, `nav2_amcl::OmniMotionModel`) as a value in the `robot_model_type` parameter, but will load the equivalent Beluga model. - -## ROS 1 Reference - -### Interface Parameters - -| Parameter | Description | Navigation AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `base_frame_id` | Robot base frame name rigidly attached to the mobile robot base. | ✅ | ✅ | -| `odom_frame_id` | Odometry frame name. The pose of a mobile platform relative to this frame can drift over time but it must be continuous (without discrete jumps). | ✅ | ✅ | -| `global_frame_id` | Map frame name. This node can estimate and publish the transform between global and odometry frames. Pose and map messages should use this coordinate frame. | ✅ | ✅ | -| `scan_topic` | The name of the topic where laser scans will be published. A transform must exist between the coordinate frame used in the scan messages and the base frame of the robot.
_A parameter that allows changing the topic name doesn't exist in AMCL, but the `scan` topic can be [remapped externally](http://wiki.ros.org/Remapping%20Arguments)._ | | ✅ | -| `map_topic` | The name of the topic where the map will be published by the map server.
_A parameter that allows changing the topic name doesn't exist in Nav AMCL, but the `map` topic can be [remapped externally](http://wiki.ros.org/Remapping%20Arguments)._ | | ✅ | -| `initial_pose_topic` | The name of the topic where an initial pose can be published.
_A parameter that allows changing the topic name doesn't exist in Nav AMCL, but the `initialpose` topic can be [remapped externally](http://wiki.ros.org/Remapping%20Arguments)._ | | ✅ | -| `use_map_topic` | Whether to retrieve the map from the configured `map_topic`, or request it through the `static_map` service instead. | ✅ | ✅ | - -### Initial Pose and Transform Broadcast Parameters - -| Parameter | Description | Navigation AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `set_initial_pose` | Whether to set initial pose from the `initial_pose*` parameters or wait for an initial pose message.
_This parameter doesn't exist in Nav AMCL. It always sets the initial pose based on available parameters._ | | ✅ | -| `initial_pose_[x, y, a]` | X, Y and yaw coordinates of initial pose of robot base frame in global frame. | ✅ | ✅ | -| `initial_pose_cov_[xx, yy, aa, xy, xa, ya]` | Covariance to use with the initial pose when initializing the particle filter.
_Nav AMCL considers off-diagonal covariances to be zero._ | ✅ | ✅ | -| `always_reset_initial_pose` | Whether to wait for an initial pose provided either via topic or `initialpose*` parameter when reset or use the last known pose to initialize.
_This parameter doesn't exist in Nav AMCL. It always uses the last known pose upon reset_. | | ✅ | -| `first_map_only` | Whether to ignore any other map messages on the `map` topic after the first one. | ✅ | ✅ | -| `save_pose_rate` | Rate (Hz) at which to store the last estimated pose and covariance to the parameter server. | ✅ | ✅ | -| `tf_broadcast` | Whether to publish the transform between the global frame and the odometry frame. The transform will be published only if an initial pose was set via topic or parameters, or if global localization was requested via the provided service. | ✅ | ✅ | -| `transform_tolerance` | Time by which to post-date the global to odom transform to indicate that it is valid in the future. | ✅ | ✅ | - -### Particle Filter Parameters - -| Parameter | Description | Navigation AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `max_particles` | Maximum allowed number of particles. | ✅ | ✅ | -| `min_particles` | Minimum allowed number of particles. | ✅ | ✅ | -| `kld_z` | Upper standard normal quantile for $P$, where $P$ is the probability that the error in the estimated distribution will be less than `pf_err` in KLD resampling [[1]](#1). | ✅ | ✅ | -| `kld_err` | Maximum particle filter population error between the true distribution and the estimated distribution. It is used in KLD resampling [[1]](#1) to limit the allowed number of particles to the minimum necessary. | ✅ | ✅ | -| `spatial_resolution_[x, y, theta]` | Spatial resolution to create buckets for KLD resampling [[1]](#1).
_These parameters don't exist in Navigation AMCL. It forces the number of buckets to be 3 times the maximum number of particles._ | | ✅ | -| `recovery_alpha_fast` | Exponential decay rate for the fast average weight filter, used in deciding when to recover from a bad approximation by adding random poses [[3]](#3). | ✅ | ✅ | -| `recovery_alpha_slow` | Exponential decay rate for the slow average weight filter, used in deciding when to recover from a bad approximation by adding random poses [[3]](#3). | ✅ | ✅ | -| `resample_interval` | Number of filter updates required before resampling. | ✅ | ✅ | -| `selective_resampling` | Whether to enable selective resampling [[2]](#2) to help avoid loss of diversity in the particle population. The resampling step will only happen if the effective number of particles $(N_{eff} = 1/ {\sum w_i^2})$ is lower than half the current number of particles, where $w_i$ refers to the normalized weight of each particle. | ✅ | ✅ | -| `update_min_a` | Rotational movement required from last resample for resampling to happen again. See [compatibility notes](#compatibility-notes-1). | ✅ | ✅ | -| `update_min_d` | Translational movement required from last resample for resampling to happen again. See [compatibility notes](#compatibility-notes-1). | ✅ | ✅ | -| `execution_policy` | Execution policy used to apply the motion update and importance weight steps to each particle (`seq` for sequential execution and `par` for parallel execution). | | ✅ | - -### Motion Model Parameters - -| Parameter | Description | Navigation AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `odom_model_type` | Which odometry motion model to use. Supported models are `diff-corrected` for differential drive [[3]](#3), `omni-corrected` for omnidirectional drive, and `stationary`. See [compatibility notes](#compatibility-notes-1). | ✅ | ✅ | -| `odom_alpha1` | Expected process noise in odometry’s rotation estimate from rotation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `odom_alpha2` | Expected process noise in odometry’s rotation estimate from translation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `odom_alpha3` | Expected process noise in odometry’s translation estimate from translation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `odom_alpha4` | Expected process noise in odometry’s translation estimate from rotation for the differential and omnidirectional drive models. | ✅ | ✅ | -| `odom_alpha5` | Expected process noise in odometry's strafe estimate from translation for the omnidirectional drive model. | ✅ | ✅ | - -### Observation Model Parameters - -| Parameter | Description | Navigation AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `laser_model_type` | Which observation model to use. Supported models are `beam` and `likelihood_field` as described in [[3]](#3) with the same aggregation formula used in Nav AMCL. | ✅ | ✅ | -| `laser_max_range` | Maximum scan range to be considered. See [compatibility notes](#compatibility-notes-1). | ✅ | ✅ | -| `laser_min_range` | Minimum scan range to be considered. See [compatibility notes](#compatibility-notes-1). | ✅ | ✅ | -| `laser_max_beams` | How many evenly spaced beams in each scan will be used when updating the filter. | ✅ | ✅ | -| `laser_sigma_hit` | Standard deviation of the hit distribution used in likelihood field and beam models. | ✅ | ✅ | -| `laser_z_hit` | Mixture weight for the probability of hitting an obstacle used in likelihood field and beam models. | ✅ | ✅ | -| `laser_z_rand` | Mixture weight for the probability of getting random measurements used in likelihood field and beam models. | ✅ | ✅ | -| `laser_z_max` | Mixture weight for the probability of getting max range measurements used in the beam model. | ✅ | ✅ | -| `laser_z_short` | Mixture weight for the probability of getting short measurements used in the beam model. | ✅ | ✅ | -| `laser_lambda_short` | Short readings' exponential distribution parameter used in the beam model. | ✅ | ✅ | -| `laser_likelihood_max_dist` | Maximum distance to do obstacle inflation on map used in the likelihood field model. | ✅ | ✅ | -| `do_beamskip` | Whether to ignore the beams for which the majority of the particles do not match the map in the likelihood field model. | ✅ | | -| `beam_skip_distance` | Maximum distance to an obstacle to consider that a beam coincides with the map. | ✅ | | -| `beam_skip_threshold` | Minimum percentage of particles for which a particular beam must match the map to not be skipped. | ✅ | | -| `beam_skip_error_threshold` | Maximum percentage of skipped beams. Too many skipped beams trigger a full update to recover in case of bad convergence. | ✅ | | - -### Diagnostics Parameters - -| Parameter | Description | Navigation AMCL | Beluga AMCL | -|-----------|-------------|:-----------------:|:-----------:| -| `std_warn_level_x` | Standard deviation upper bound for pose x position estimates before triggering a warning. | ✅ | ✅ | -| `std_warn_level_y` | Standard deviation upper bound for pose y position estimates before triggering a warning. | ✅ | ✅ | -| `std_warn_level_yaw` | Standard deviation upper bound for pose yaw rotation estimates before triggering a warning. | ✅ | ✅ | - -### Compatibility Notes - -- Beluga AMCL supports `diff-corrected` and `omni-corrected` motion models, older `diff` and `omni` models from Navigation AMCL were not implemented. -- Unlike Navigation AMCL, Beluga AMCL needs both `laser_max_range` and `laser_min_range` to be specified (ie. -1.0 is not a valid value). - -## Additional Notes - -- Unless otherwise specified, all units are [REP-103](https://www.ros.org/reps/rep-0103.html) compliant. -- Coordinate frames should follow conventions specified in [REP-105](https://www.ros.org/reps/rep-0105.html). - -## References - -[1] Dieter Fox. Kld-sampling: Adaptive particle filters. In Proceedings of the 14th International Conference on Neural Information Processing Systems: Natural and Synthetic, NIPS'01, pages 713–720, Cambridge, MA, USA, 2001. MIT Press. https://dl.acm.org/doi/10.5555/2980539.2980632 - -[2] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard. Improved techniques for grid mapping with rao-blackwellized particle filters. IEEE Transactions on Robotics, 23(1):34–46, 2007. https://doi.org/10.1109/TRO.2006.889486 - -[3] S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. Intelligent Robotics and Autonomous Agents series. MIT Press, 2005. https://books.google.com.ar/books?id=jtSMEAAAQBAJ diff --git a/beluga_amcl/docs/_doxygen/beluga_amcl-main.md b/beluga_amcl/docs/_doxygen/beluga_amcl-main.md new file mode 100644 index 000000000..392248c93 --- /dev/null +++ b/beluga_amcl/docs/_doxygen/beluga_amcl-main.md @@ -0,0 +1,19 @@ +# API Reference + +This is the API reference for Beluga AMCL. It provides reusable ROS and ROS 2 nodes. + +### Components + +Explore the library's components: + +#### ROS 2 Nodes + +| | | +|-|-| +| beluga_amcl::AmclNode | 2D AMCL as a ROS 2 composable lifecycle node, feature compatible with [Nav2 AMCL](https://index.ros.org/p/nav2_amcl). | + +#### ROS 1 Nodelets + +| | | +|-|-| +| beluga_amcl::AmclNodelet | 2D AMCL as a ROS (1) nodelet, feature compatible with [AMCL](https://github.com/ros-planning/navigation/tree/noetic-devel/amcl). | diff --git a/beluga_amcl/docs/_images b/beluga_amcl/docs/_images new file mode 120000 index 000000000..097050528 --- /dev/null +++ b/beluga_amcl/docs/_images @@ -0,0 +1 @@ +../../docs/_images \ No newline at end of file diff --git a/beluga_amcl/docs/_static b/beluga_amcl/docs/_static new file mode 120000 index 000000000..7fdcaa74f --- /dev/null +++ b/beluga_amcl/docs/_static @@ -0,0 +1 @@ +../../docs/_static \ No newline at end of file diff --git a/beluga_amcl/docs/conf.py b/beluga_amcl/docs/conf.py new file mode 100644 index 000000000..d6a32713e --- /dev/null +++ b/beluga_amcl/docs/conf.py @@ -0,0 +1,84 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Configuration file for the Sphinx documentation builder. +# +# For the full list of built-in configuration values, see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +import subprocess + +# -- Project information ----------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information + +project = "Beluga AMCL" +copyright = "2022-2024 Ekumen, Inc." +author = "Ekumen Research" + +# -- General configuration --------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration +source_suffix = { + ".rst": "restructuredtext", + ".md": "markdown", +} +language = "en" + +extensions = [ + "myst_parser", + "sphinx.ext.mathjax", + "sphinx.ext.duration", + "sphinx.ext.graphviz", + "sphinx_copybutton", + "sphinx.ext.inheritance_diagram", + "sphinx_design", + "sphinx_babel.autodox", + "sphinxcontrib.bibtex", +] + +bibtex_bibfiles = ["references.bib"] + +myst_enable_extensions = ["colon_fence"] +myst_heading_anchors = 4 + +autodox_outdir = "_doxygen/generated" +autodox_projects = {"reference": ""} + +exclude_patterns = ["README.md", "_build", "_doxygen/*.md"] +suppress_warnings = ["myst.header"] + +# -- Options for HTML output ------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output + +html_theme = "sphinx_book_theme" +html_theme_options = { + "show_navbar_depth": 1, + "collapse_navbar": True, + "collapse_navigation": True, + "repository_branch": "main", + "repository_url": "https://github.com/Ekumen-OS/beluga", + "use_repository_button": True, + "use_edit_page_button": False, + "home_page_in_toc": True, + "logo": { + "image_light": "_images/logo_with_name_light.png", + "image_dark": "_images/logo_with_name_dark.png", + }, +} +html_last_updated_fmt = subprocess.check_output( + ["git", "log", "--pretty=format:'%ad, %h'", "-n1"] +).decode("utf-8") +html_favicon = "_images/logo_200x200.png" +html_css_files = ["custom.css"] +html_js_files = ["custom.js"] +html_static_path = ["_static"] diff --git a/beluga_amcl/docs/index.md b/beluga_amcl/docs/index.md new file mode 100644 index 000000000..eefe95753 --- /dev/null +++ b/beluga_amcl/docs/index.md @@ -0,0 +1,14 @@ +# Overview + +```{toctree} +:hidden: +:maxdepth: 1 + +ros-reference +ros2-reference +API Reference <_doxygen/generated/reference/html/index> +``` + +Beluga AMCL is a ROS and ROS 2 package that builds on Beluga to provide 2D AMCL for mobile robots. +It is interface and feature wise compatible with both of its longstanding counterparts in the ROS ecosystem: [`amcl`](https://github.com/ros-planning/navigation/tree/noetic-devel/amcl) and +[`nav2_amcl`](https://github.com/ros-planning/navigation2/tree/main/nav2_amcl) packages. This affords a simple migration path for `nav2_amcl`-based (or `amcl`-based) projects that want to leverage Beluga instead. diff --git a/beluga_amcl/docs/references.bib b/beluga_amcl/docs/references.bib new file mode 100644 index 000000000..9dd1549d5 --- /dev/null +++ b/beluga_amcl/docs/references.bib @@ -0,0 +1,67 @@ +@book{thrun2005probabilistic, + title={Probabilistic Robotics}, + author={Thrun, S. and Burgard, W. and Fox, D.}, + isbn={9780262201629}, + lccn={20050436}, + series={Intelligent Robotics and Autonomous Agents series}, + url={https://books.google.com.ar/books?id=jtSMEAAAQBAJ}, + year={2005}, + publisher={MIT Press} +} + +@book{gentle2009computationalstatistics, + title={Computational Statistics}, + author={Gentle, J. E.}, + isbn={9780387981437}, + series={Statistics and Computing}, + year={2009}, + publisher={Springer}, + doi={10.1007/978-0-387-98144-4} +} + +@article{grisetti2007selectiveresampling, + author={Grisetti, Giorgio and Stachniss, Cyrill and Burgard, Wolfram}, + journal={IEEE Transactions on Robotics}, + title={Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters}, + year={2007}, + volume={23}, + number={1}, + pages={34-46}, + doi={10.1109/TRO.2006.889486} +} + +@article{tiacheng2015resamplingmethods, + author={Li, Tiancheng and Bolic, Miodrag and Djuric, Petar M.}, + journal={IEEE Signal Processing Magazine}, + title={Resampling Methods for Particle Filtering: Classification, implementation, and strategies}, + year={2015}, + volume={32}, + number={3}, + pages={70-86}, + doi={10.1109/MSP.2014.2330626} +} + +@inproceedings{fox2001adaptivekldsampling, + author={Fox, Dieter}, + title={KLD-Sampling: Adaptive Particle Filters}, + year={2001}, + publisher={MIT Press}, + address={Cambridge, MA, USA}, + booktitle={Proceedings of the 14th International Conference on Neural Information Processing Systems: Natural and Synthetic}, + pages={713-720}, + numpages={8}, + location={Vancouver, British Columbia, Canada}, + series = {NIPS'01}, + doi={10.5555/2980539.2980632} +} + +@article{kong1994sequentialimputations, + auhor={Kong, A., Liu, J. S., & Wong, W. H.}, + journal={Journal of the American Statistical Association}, + title={Sequential Imputations and Bayesian Missing Data Problems}, + year={1994}, + volume={89}, + number={425}, + pages={278-288}, + doi={10.1080/01621459.1994.10476469} +} diff --git a/beluga_amcl/docs/ros-reference.md b/beluga_amcl/docs/ros-reference.md new file mode 100644 index 000000000..fc4ec1ac8 --- /dev/null +++ b/beluga_amcl/docs/ros-reference.md @@ -0,0 +1,271 @@ +# ROS Reference + +## Nodelets + +### beluga\_amcl/AmclNodelet + +2D AMCL as a nodelet. The nodelet is implemented as a thin wrapper, in charge of managing ROS communication, configuration, data conversion, +and ROS node initialization and shutdown, built around a single ROS agnostic Beluga particle filter. + +Also available as a standalone `amcl_node` executable. + +#### Parameters + +##### Interface Parameters + +`base_frame_id` _(`string`)_ +: Robot base frame name rigidly attached to the mobile robot base. +: Defaults to `base_footprint`. + +`odom_frame_id` _(`string`)_ +: Odometry frame name. The pose of a mobile platform relative to this frame can drift over time but it must be continuous (without discrete jumps). +: Defaults to `odom`. + +`global_frame_id` _(`string`)_ +: Map frame name. This node can estimate and publish the transform between global and odometry frames. Pose and map messages should use this coordinate frame. +: Defaults to `map`. + +`scan_topic` _(`string`)_ +: The name of the topic where laser scans will be published. A transform must exist between the coordinate frame used in the scan messages and the base frame of the robot. +: Defaults to `scan`. + +`map_topic` _(`string`)_ +: The name of the topic to subscribe to for map updates. Typically published by the [map server](https://github.com/ros-planning/navigation2/tree/main/nav2_map_server). +: Defaults to `map`. + +`map_service` _(`string`)_ +: The name of the service to request map updates to. +: Defaults to `static_map`. + +`use_map_topic` _(`boolean`)_ +: Whether to use a map topic or a map service for filter (re)initialization. +: Defaults to `true`. + +`initial_pose_topic` _(`string`)_ +: The name of the topic where an initial pose can be published. +: Defaults to `initialpose`. + +##### Initial Pose and Transform Broadcast Parameters + +`set_initial_pose` _(`boolean`)_ +: Whether to set initial pose from the `initial_pose*` parameters or wait for an initial pose message. +: Defaults to `false`. + +`initial_pose_[x, y, a]` _(`float`)_ +: X, Y and yaw coordinates of initial pose of robot base frame in global frame. +: Defaults to `0.0`. + +`initial_cov_[xx, yy, aa, xy, xa, ya]` _(`float`)_ +: Covariance to use with the initial pose when initializing the particle filter. +: Defaults to `0.0`. + +`always_reset_initial_pose` _(`boolean`)_ +: Whether to wait for an initial pose provided either via topic or `initial_pose*` parameter when reset or use the last known pose to initialize. +: Defaults to `false`. + +`save_pose_rate` _(`boolean`)_ +: Rate at which to store the last estimated pose and covariance to the parameter server. +: Defaults to `0.5`. + +`first_map_only` _(`boolean`)_ +: Whether to ignore any other map messages on the `map` topic after the first one. +: Defaults to `false`. + +`tf_broadcast` _(`boolean`)_ +: Whether to publish the transform between the global frame and the odometry frame. The transform will be published only if an initial pose was set via topic or parameters, or if global localization was requested via the provided service. +: Defaults to `true`. + +`transform_tolerance` _(`float`)_ +: Time lapse, in seconds, by which to post-date the global to odom transform to indicate that it is valid in the future. +: Defaults to `1.0`. + +##### Particle Filter Parameters + +`max_particles` _(`integer`)_ +: Maximum allowed number of particles. +: Defaults to `5000`. + +`min_particles` _(`integer`)_ +: Minimum allowed number of particles. +: Defaults to `100`. + +`pf_z` _(`float`)_ +: Upper standard normal quantile for $P$, where $P$ is the probability that the error in the estimated distribution will be less than `pf_err` in KLD resampling {cite}`fox2001adaptivekldsampling`. +: Defaults to `0.99`. + +`pf_err` _(`float`)_ +: Maximum particle filter population error between the true distribution and the estimated distribution. It is used in KLD resampling {cite}`fox2001adaptivekldsampling` to limit the allowed number of particles to the minimum necessary. +: Defaults to `0.05`. + +`spatial_resolution_[x, y, theta]` _(`float`)_ +: Spatial resolution to create buckets for KLD resampling {cite}`fox2001adaptivekldsampling`. +: Defaults to `0.5` for translation and `10°` for rotation. + +`recovery_alpha_fast` _(`float`)_ +: Exponential decay rate for the fast average weight filter, used in deciding when to recover from a bad approximation by adding random poses {cite}`thrun2005probabilistic`. +: Defaults to `0.001`. + +`recovery_alpha_slow` _(`float`)_ +: Exponential decay rate for the slow average weight filter, used in deciding when to recover from a bad approximation by adding random poses {cite}`thrun2005probabilistic`. +: Defaults to `0.1`. + +`resample_interval` _(`integer`)_ +: Number of filter updates required before resampling. +: Defaults to `2`. + +`selective_resampling` _(`boolean`, read-only)_ +: Whether to enable selective resampling {cite}`grisetti2007selectiveresampling` to help avoid loss of diversity in the particle population. The resampling step will only happen if the effective number of particles $(N_{eff} = 1/ {\sum w_i^2})$ is lower than half the current number of particles, where $w_i$ refers to the normalized weight of each particle. +: Defaults to `false`. + +`update_min_a` _(`float`)_ +: Minimum rotation required from last resample for resampling to happen again. Must be in the $[0, 2\pi]$ interval. +: Defaults to `0.2`. + +`update_min_d` _(`float`)_ +: Minimum translation required from last resample for resampling to happen again. Must be nonnegative. +: Defaults to `0.523598` ($\pi / 6$). + +`execution_policy` _(`string`)_ +: Execution policy used to apply the motion update and importance weight steps to each particle. `seq` for sequential execution and `par` for parallel execution. +: Defaults to `seq`. + +##### Motion Model Parameters + +`robot_model_type` _(`string`)_ +: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive` and `stationary`. +: Defaults to `differential_drive`. + +`odom_alpha1` _(`float`)_ +: Expected process noise in odometry’s rotation estimate from rotation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`odom_alpha2` _(`float`)_ +: Expected process noise in odometry’s rotation estimate from translation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`odom_alpha3` _(`float`)_ +: Expected process noise in odometry’s translation estimate from translation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`odom_alpha4` _(`float`)_ +: Expected process noise in odometry’s translation estimate from rotation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`odom_alpha5` _(`float`)_ +: Expected process noise in odometry's strafe estimate from translation for the `omnidirectional_drive` model. Must be nonnegative. +: Defaults to `0.2`. + +##### Observation Model Parameters + +`laser_model_type` _(`string`)_ +: Which observation model to use. Supported models are `beam` and `likelihood_field` as described in {cite}`thrun2005probabilistic` with the same aggregation formula used in Nav2 AMCL. +: Defaults to `likelihood_field`. + +`laser_max_range` _(`float`)_ +: Maximum scan range to be considered. Must be nonnegative. +: Defaults to `100.0`. + +`laser_min_range` _(`float`)_ +: Minimum scan range to be considered. Must be nonnegative. +: Defaults to `0.0`. + +`max_beams` _(`integer`)_ +: How many evenly spaced beams in each scan will be used when updating the filter. +: Defaults to `60`. + +`laser_sigma_hit` _(`float`)_ +: Standard deviation of the hit distribution used in `likelihood_field` and `beam` models. +: Defaults to `0.2`. + +`laser_z_hit` _(`float`)_ +: Mixture weight for the probability of hitting an obstacle used in `likelihood_field` and `beam` models. +: Defaults to `0.5`. + +`laser_z_rand` _(`float`)_ +: Mixture weight for the probability of getting random measurements used in `likelihood_field` and `beam` models. +: Defaults to `0.5`. + +`laser_z_max` _(`float`)_ +: Mixture weight for the probability of getting max range measurements used in the `beam` model. +: Defaults to `0.05`. + +`laser_z_short` _(`float`)_ +: Mixture weight for the probability of getting short measurements used in the `beam` model. +: Defaults to `0.05`. + +`laser_lambda_short` _(`float`)_ +: Short readings' exponential distribution parameter used in the `beam` model. +: Defaults to `0.1`. + +`laser_likelihood_max_dist` _(`float`)_ +: Maximum distance, in meters, to do obstacle inflation on map used in the `likelihood_field` model. +: Defaults to `2.0`. + +##### Misc Parameters + +`std_warn_level_[x, y, yaw]` +: Threshold on the standard deviation of the x, y, and yaw coordinates of the pose estimate before emitting warnings.\ +: Defaults to `0.2`. + +`restore_defaults` _(`boolean`)_ +: Flag to restore parameter defaults. Reset (to `false`) upon restoration. +: Defaults to `false`. + +### Published topics + +`particlecloud` +: Estimated pose distribution published as `geometry_msgs/PoseArray` messages. It will only be published if subscribers are found. + +`amcl_pose` +: Mean and covariance of the estimated pose distribution published as `geometry_msgs/PoseWithCovarianceStamped` messages (assumed Gaussian). + +`/diagnostics` +: Filter [diagnostics](https://wiki.ros.org/diagnostics) published as `diagnostics_msgs/DiagnosticArray` messages. + +### Subscribed topics + +`` +: Occupancy grid map updates subscribed as `nav_msgs/OccupancyGrid` messages. Actual topic name is dictated by the `map_topic` parameter. +: Only subscribed if `use_map_topic` is `true`. + +`` +: Gaussian pose distribution subscribed as `geometry_msgs/PoseWithCovarianceStamped` messages for filter (re)initialization. Actual topic name is dictated by the `initial_pose_topic` parameter. + +`` +: Lidar scan updates subscribed as `sensor_msgs/LaserScan` messages. Actual topic name is dictated by the `scan_topic` parameter. + +### Subscribed transforms + +`` → `` +: Odometry estimates as transforms from the configured odometry frame to the configured base frame. Used by motion models and resampling policies. Actual frame IDs are dictated by `odom_frame_id` and `base_frame_id` parameters. + +`` → `scan_frame_id` +: Lidar extrinsics as transforms from the configured base frame to the lidar scan frame. Actual frame IDs are dictated by the `base_frame_id` parameter and `header.frame_id` member in `scan_topic` messages. + +### Broadcasted transforms + +`` → `` +: Transforms from the configured global frame to the configured odometry frame, calculated such that when composed with the corresponding odometry estimate, the mean of the estimated pose distribution in the global frame results. Actual frame IDs are dictated by `global_frame_id` and `odom_frame_id` parameters. +: Only broadcasted if `tf_broadcast` is set to `true`. + +### Advertised services + +`global_localization` +: An `std_srvs/Empty` service to force a filter (re)initialization by sampling a uniform pose distribution over the last known map. + +`request_nomotion_update` +: An `std_srvs/Empty` service to force a filter update upon request. + +`set_map` +: A `nav_msgs/SetMap` service to force a map update. + +### Called services + +`` +: A `nav_msgs/GetMap` service to get the first map from. Actual service name is dictated by the `map_service` parameter. +: Only called if `use_map_topic` is `false`. + +### Compatibility Notes + +- Beluga AMCL supports `diff-corrected` and `omni-corrected` motion models. Older `diff` and `omni` models from Navigation AMCL were not implemented. +- Unlike Navigation AMCL, Beluga AMCL needs both `laser_max_range` and `laser_min_range` to be specified (ie. -1.0 is not a valid value). diff --git a/beluga_amcl/docs/ros2-reference.md b/beluga_amcl/docs/ros2-reference.md new file mode 100644 index 000000000..e45ac46ad --- /dev/null +++ b/beluga_amcl/docs/ros2-reference.md @@ -0,0 +1,290 @@ +# ROS 2 Reference + +## Nodes + +### beluga_amcl::AmclNode + +2D AMCL as a composable lifecycle node, with a [bond](https://github.com/ros/bond_core/tree/ros2) with a lifecycle manager. +The node is implemented as a thin wrapper, in charge of managing ROS 2 communication, configuration, data conversion, +and ROS 2 node initialization and shutdown, built around a single ROS 2 agnostic Beluga particle filter. + +Also available as a standalone `amcl_node` executable. + +#### Parameters + +##### Interface Parameters + +`base_frame_id` _(`string`)_ +: Robot base frame name rigidly attached to the mobile robot base. +: Defaults to `base_footprint`. + +`odom_frame_id` _(`string`)_ +: Odometry frame name. The pose of a mobile platform relative to this frame can drift over time but it must be continuous (without discrete jumps). +: Defaults to `odom`. + +`global_frame_id` _(`string`)_ +: Map frame name. This node can estimate and publish the transform between global and odometry frames. Pose and map messages should use this coordinate frame. +: Defaults to `map`. + +`scan_topic` _(`string`)_ +: The name of the topic where laser scans will be published. A transform must exist between the coordinate frame used in the scan messages and the base frame of the robot. +: Defaults to `scan`. + +`map_topic` _(`string`)_ +: The name of the topic to subscribe to for map updates. Typically published by the [map server](https://github.com/ros-planning/navigation2/tree/main/nav2_map_server). +: Defaults to `map`. + +`initial_pose_topic` _(`string`)_ +: The name of the topic where an initial pose can be published. +: Defaults to `initialpose`. + +##### Initial Pose and Transform Broadcast Parameters + +`set_initial_pose` _(`boolean`)_ +: Whether to set initial pose from the `initial_pose*` parameters or wait for an initial pose message. +: Defaults to `false`. + +`initial_pose.[x, y, yaw]` _(`float`)_ +: X, Y and yaw coordinates of initial pose of robot base frame in global frame. +: Defaults to `0.0`. + +`initial_pose.covariance_[x, y, yaw, xy, xyaw, yyaw]` _(`float`)_ +: Covariance to use with the initial pose when initializing the particle filter. +: Defaults to `0.0`. + +`always_reset_initial_pose` _(`boolean`)_ +: Whether to wait for an initial pose provided either via topic or `initial_pose*` parameter when reset or use the last known pose to initialize. +: Defaults to `false`. + +`first_map_only` _(`boolean`)_ +: Whether to ignore any other map messages on the `map` topic after the first one. +: Defaults to `false`. + +`tf_broadcast` _(`boolean`)_ +: Whether to publish the transform between the global frame and the odometry frame. The transform will be published only if an initial pose was set via topic or parameters, or if global localization was requested via the provided service. +: Defaults to `true`. + +`transform_tolerance` _(`float`)_ +: Time lapse, in seconds, by which to post-date the global to odom transform to indicate that it is valid in the future. +: Defaults to `1.0`. + +##### Particle Filter Parameters + +`max_particles` _(`integer`)_ +: Maximum allowed number of particles. +: Defaults to `2000`. + +`min_particles` _(`integer`)_ +: Minimum allowed number of particles. +: Defaults to `500`. + +`pf_z` _(`float`)_ +: Upper standard normal quantile for $P$, where $P$ is the probability that the error in the estimated distribution will be less than `pf_err` in KLD resampling {cite}`fox2001adaptivekldsampling`. +: Defaults to `0.99`. + +`pf_err` _(`float`)_ +: Maximum particle filter population error between the true distribution and the estimated distribution. It is used in KLD resampling {cite}`fox2001adaptivekldsampling` to limit the allowed number of particles to the minimum necessary. +: Defaults to `0.05`. + +`spatial_resolution_[x, y, theta]` _(`float`)_ +: Spatial resolution to create buckets for KLD resampling {cite}`fox2001adaptivekldsampling`. +: Defaults to `0.5` for translation and `10°` for rotation. + +`recovery_alpha_fast` _(`float`)_ +: Exponential decay rate for the fast average weight filter, used in deciding when to recover from a bad approximation by adding random poses {cite}`thrun2005probabilistic`. +: Defaults to `0.0`. + +`recovery_alpha_slow` _(`float`)_ +: Exponential decay rate for the slow average weight filter, used in deciding when to recover from a bad approximation by adding random poses {cite}`thrun2005probabilistic`. +: Defaults to `0.0`. + +`resample_interval` _(`integer`)_ +: Number of filter updates required before resampling. +: Defaults to `1`. + +`selective_resampling` _(`boolean`, read-only)_ +: Whether to enable selective resampling {cite}`grisetti2007selectiveresampling` to help avoid loss of diversity in the particle population. The resampling step will only happen if the effective number of particles $(N_{eff} = 1/ {\sum w_i^2})$ is lower than half the current number of particles, where $w_i$ refers to the normalized weight of each particle. +: Defaults to `false`. + +`update_min_a` _(`float`)_ +: Minimum rotation required from last resample for resampling to happen again. Must be in the $[0, 2\pi]$ interval. +: Defaults to `0.2`. + +`update_min_d` _(`float`)_ +: Minimum translation required from last resample for resampling to happen again. Must be nonnegative. +: Defaults to `0.25`. + +`execution_policy` _(`string`)_ +: Execution policy used to apply the motion update and importance weight steps to each particle. `seq` for sequential execution and `par` for parallel execution. +: Defaults to `seq`. + +##### Motion Model Parameters + +`robot_model_type` _(`string`)_ +: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive` and `stationary`. +: Defaults to `differential_drive`. + +`alpha1` _(`float`)_ +: Expected process noise in odometry’s rotation estimate from rotation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`alpha2` _(`float`)_ +: Expected process noise in odometry’s rotation estimate from translation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`alpha3` _(`float`)_ +: Expected process noise in odometry’s translation estimate from translation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`alpha4` _(`float`)_ +: Expected process noise in odometry’s translation estimate from rotation for the `differential_drive` and `omnidirectional_drive` models. Must be nonnegative. +: Defaults to `0.2`. + +`alpha5` _(`float`)_ +: Expected process noise in odometry's strafe estimate from translation for the `omnidirectional_drive` model. Must be nonnegative. +: Defaults to `0.2`. + +##### Observation Model Parameters + +`laser_model_type` _(`string`)_ +: Which observation model to use. Supported models are `beam` and `likelihood_field` as described in {cite}`thrun2005probabilistic` with the same aggregation formula used in Nav2 AMCL. +: Defaults to `likelihood_field`. + +`laser_max_range` _(`float`)_ +: Maximum scan range to be considered. Must be nonnegative. +: Defaults to `100.0`. + +`laser_min_range` _(`float`)_ +: Minimum scan range to be considered. Must be nonnegative. +: Defaults to `0.0`. + +`max_beams` _(`integer`)_ +: How many evenly spaced beams in each scan will be used when updating the filter. +: Defaults to `60`. + +`sigma_hit` _(`float`)_ +: Standard deviation of the hit distribution used in `likelihood_field` and `beam` models. +: Defaults to `0.2`. + +`z_hit` _(`float`)_ +: Mixture weight for the probability of hitting an obstacle used in `likelihood_field` and `beam` models. +: Defaults to `0.5`. + +`z_rand` _(`float`)_ +: Mixture weight for the probability of getting random measurements used in `likelihood_field` and `beam` models. +: Defaults to `0.5`. + +`z_max` _(`float`)_ +: Mixture weight for the probability of getting max range measurements used in the `beam` model. +: Defaults to `0.05`. + +`z_short` _(`float`)_ +: Mixture weight for the probability of getting short measurements used in the `beam` model. +: Defaults to `0.05`. + +`lambda_short _(`float`)_ +: Short readings' exponential distribution parameter used in the `beam` model. +: Defaults to `0.1`. + +`laser_likelihood_max_dist` _(`float`)_ +: Maximum distance, in meters, to do obstacle inflation on map used in the `likelihood_field` model. +: Defaults to `2.0`. + +### Published topics + +`particle_cloud` +: Estimated pose distribution published as `nav2_msgs/msg/ParticleCloud` messages, using a sensor data QoS policy. It will only be published if subscribers are found. + +`pose` +: Mean and covariance of the estimated pose distribution published as `geometry_msgs/msg/PoseWithCovarianceStamped` messages (assumed Gaussian), using a system default QoS policy. + +### Subscribed topics + +`` +: Occupancy grid map updates subscribed as `nav_msgs/msg/OccupancyGrid` messages, using a reliable transient local QoS policy with keep last of 1 (ie. single message latching). Actual topic name is dictated by the `map_topic` parameter. +: Only subscribed if `use_map_topic` is `true`. + +: Occupancy grid map subscribed for sensor models to work with. + +`` +: Gaussian pose distribution subscribed as `geometry_msgs/msg/PoseWithCovarianceStamped` messages, using a system default QoS policy, for filter (re)initialization. Actual topic name is dictated by the `initial_pose_topic` parameter. + +`` +: Lidar scan updates subscribed as `sensor_msgs/msg/LaserScan` messages, using a sensor data QoS policy. Actual topic name is dictated by the `scan_topic` parameter. +: Lidar scans subscribed for sensor models to work with. + +### Subscribed transforms + +`` → `` +: Odometry estimates as transforms from the configured odometry frame to the configured base frame. Used by motion models and resampling policies. Actual frame IDs are dictated by `odom_frame_id` and `base_frame_id` parameters. + +`` → `scan_frame_id` +: Lidar extrinsics as transforms from the configured base frame to the lidar scan frame. Actual frame IDs are dictated by the `base_frame_id` parameter and `header.frame_id` member in `scan_topic` messages. + +### Broadcasted transforms + +`` → `` +: Transforms from the configured global frame to the configured odometry frame, calculated such that when composed with the corresponding odometry estimate, the mean of the estimated pose distribution in the global frame results. Actual frame IDs are dictated by `global_frame_id` and `odom_frame_id` parameters. +: Only broadcasted if `tf_broadcast` is set to `true`. + +### Advertised services + +`reinitialize_global_localization` +: An `std_srvs/srv/Empty` service, using a default service QoS policy, to force a filter (re)initialization by sampling a uniform pose distribution over the last known map. + +`request_nomotion_update` +: An `std_srvs/srv/Empty` service, using a default service QoS policy, to force a filter update upon request. + +## Compatibility notes + +- Beluga AMCL supports Nav2 AMCL plugin names (`nav2_amcl::DifferentialMotionModel`, `nav2_amcl::OmniMotionModel`) as a value in the `robot_model_type` parameter, but will load the equivalent Beluga model. +- Notes on parameter and feature availability between Beluga AMCL and Nav2 AMCL are condensed in the table below. + +| Parameter | Notes | Navigation 2 AMCL | Beluga AMCL | | +|---|---|:---:|:---:|---| +| `base_frame_id` | | ✅ | ✅ | | +| `odom_frame_id` | | ✅ | ✅ | | +| `global_frame_id` | | ✅ | ✅ | | +| `scan_topic` | | ✅ | ✅ | | +| `map_topic` | | ✅ | ✅ | | +| `initial_pose_topic` | A parameter that allows changing the topic name doesn't exist in Nav2 AMCL, but the `initialpose` topic can be [remapped externally](https://design.ros2.org/articles/static_remapping.html). | | ✅ | | +| `set_initial_pose` | | ✅ | ✅ | | +| `initial_pose.[x, y, yaw]` | | ✅ | ✅ | | +| `initial_pose.covariance_[x, y, yaw, xy, xyaw, yyaw]` | Nav2 AMCL considers these to be zero. | | ✅ | | +| `always_reset_initial_pose` | | ✅ | ✅ | | +| `first_map_only` | | ✅ | ✅ | | +| `tf_broadcast` | | ✅ | ✅ | | +| `transform_tolerance` | | ✅ | ✅ | | +| `max_particles` | | ✅ | ✅ | | +| `min_particles` | | ✅ | ✅ | | +| `pf_z` | | ✅ | ✅ | | +| `pf_err` | | ✅ | ✅ | | +| `spatial_resolution_[x, y, theta]` | | | ✅ | | +| `recovery_alpha_fast` | | ✅ | ✅ | | +| `recovery_alpha_slow` | | ✅ | ✅ | | +| `resample_interval` | | ✅ | ✅ | | +| `selective_resampling` | This feature is currently supported by Nav AMCL in ROS 1 but it hasn't been ported to ROS 2 at the time of this writing. | | ✅ | | +| `update_min_a` | | ✅ | ✅ | | +| `update_min_d` | | ✅ | ✅ | | +| `execution_policy` | | | ✅ | | +| `robot_model_type` | Beluga AMCL supports Nav2 AMCL plugin names (`nav2_amcl::DifferentialMotionModel`, `nav2_amcl::OmniMotionModel`) as a value in the `robot_model_type` parameter, but will load the equivalent Beluga model. | ✅ | ✅ | | +| `alpha1` | | ✅ | ✅ | | +| `alpha2` | | ✅ | ✅ | | +| `alpha3` | | ✅ | ✅ | | +| `alpha4` | | ✅ | ✅ | | +| `alpha5` | | ✅ | ✅ | | +| `laser_model_type` | | ✅ | ✅ | | +| `laser_max_range` | | ✅ | ✅ | | +| `laser_min_range` | | ✅ | ✅ | | +| `max_beams` | | ✅ | ✅ | | +| `sigma_hit` | | ✅ | ✅ | | +| `z_hit` | | ✅ | ✅ | | +| `z_rand` | | ✅ | ✅ | | +| `z_max` | | ✅ | ✅ | | +| `z_short` | | ✅ | ✅ | | +| `lambda_short` | | ✅ | ✅ | | +| `laser_likelihood_max_dist` | | ✅ | ✅ | | +| `do_beamskip` | Whether to ignore the beams for which the majority of the particles do not match the map in the likelihood field model. Beluga AMCL does not support beam skipping. | ✅ | | | +| `beam_skip_distance` | Maximum distance to an obstacle to consider that a beam coincides with the map. Beluga AMCL does not support beam skipping. | ✅ | | | +| `beam_skip_threshold` | Minimum percentage of particles for which a particular beam must match the map to not be skipped. Beluga AMCL does not support beam skipping. | ✅ | | | +| `beam_skip_error_threshold` | Maximum percentage of skipped beams. Too many skipped beams trigger a full update to recover in case of bad convergence. Beluga AMCL does not support beam skipping. | ✅ | | | diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index 0f6a9f2e5..d9d6687e3 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -36,50 +36,73 @@ #include #include +/** + * \file + * \brief ROS 2 integration of the 2D AMCL algorithm. + */ + namespace beluga_amcl { +/// 2D AMCL as a ROS 2 composable lifecycle node. class AmclNode : public rclcpp_lifecycle::LifecycleNode { public: using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + /// Constructor. explicit AmclNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); ~AmclNode() override; protected: + /// Callback for lifecycle transitions from the UNCONFIGURED state to the INACTIVE state. CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; + /// Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state. CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; + /// Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state. CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; + /// Callback for lifecycle transitions from the INACTIVE state to the UNCONFIGURED state. CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; + /// Callback for lifecycle transitions from most states to the FINALIZED state. CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override; + /// Get initial pose estimate from parameters if set. auto get_initial_estimate() const -> std::optional>; + /// Get motion model as per current parametrization. auto get_motion_model(std::string_view) const -> beluga_ros::Amcl::motion_model_variant; + /// Get sensor model as per current parametrization. auto get_sensor_model(std::string_view, nav_msgs::msg::OccupancyGrid::SharedPtr) const -> beluga_ros::Amcl::sensor_model_variant; + /// Get execution policy given its name. static auto get_execution_policy(std::string_view) -> beluga_ros::Amcl::execution_policy_variant; + /// Instantiate particle filter given an initial occupancy grid map and the current parametrization. auto make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr) const -> std::unique_ptr; + /// Callback for occupancy grid map updates. void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr); + /// Callback for periodic particle cloud updates. void timer_callback(); + /// Callback for laser scan updates. void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr); + /// Callback for pose (re)initialization. void initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + /// Callback for the global relocalization service. void global_localization_callback( std::shared_ptr request_header, std::shared_ptr request, std::shared_ptr response); + /// Callback for the no motion update service. void nomotion_update_callback( std::shared_ptr request_header, std::shared_ptr req, @@ -106,31 +129,46 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode { */ bool initialize_from_map(); + /// Node bond with the lifecycle manager. std::unique_ptr bond_; + + /// Timer for periodic particle cloud updates. rclcpp::TimerBase::SharedPtr timer_; - // Publishers + /// Particle cloud publisher. rclcpp_lifecycle::LifecyclePublisher::SharedPtr particle_cloud_pub_; + /// Estimated pose publisher. rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; - // Subscribers + /// Pose (re)initialization subscription. rclcpp::Subscription::SharedPtr initial_pose_sub_; + /// Occupancy grid map updates subscription. rclcpp::Subscription::SharedPtr map_sub_; + /// Laser scan updates subscription. std::unique_ptr> laser_scan_sub_; - // Services + /// Global relocalization service server. rclcpp::Service::SharedPtr global_localization_server_; + /// No motion update service server. rclcpp::Service::SharedPtr nomotion_update_server_; + /// Transforms buffer. std::unique_ptr tf_buffer_; + /// Transforms broadcaster. std::unique_ptr tf_broadcaster_; + /// Transforms listener. std::unique_ptr tf_listener_; + /// Transform synchronization filter for laser scan updates. std::unique_ptr> laser_scan_filter_; + /// Connection for laser scan updates filter and callback. message_filters::Connection laser_scan_connection_; + /// Particle filter instance. std::unique_ptr particle_filter_; + /// Last known pose estimate, if any. std::optional> last_known_estimate_; + /// Whether to broadcast transforms or not. bool enable_tf_broadcast_{false}; }; diff --git a/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp b/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp index 07d1381a8..49efb1117 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp @@ -44,47 +44,73 @@ #include #include +/** + * \file + * \brief ROS (1) integration of the 2D AMCL algorithm. + */ + namespace beluga_amcl { +/// 2D AMCL as a ROS (1) nodelet. class AmclNodelet : public nodelet::Nodelet { public: AmclNodelet() = default; ~AmclNodelet() override = default; protected: + /// Callback for nodelet initialization. void onInit() override; + /// Get initial pose estimate from parameters if set. auto get_initial_estimate() const -> std::optional>; + /// Get motion model as per current parametrization. auto get_motion_model(std::string_view) const -> beluga_ros::Amcl::motion_model_variant; + /// Get sensor model as per current parametrization. auto get_sensor_model(std::string_view, const nav_msgs::OccupancyGrid::ConstPtr&) const -> beluga_ros::Amcl::sensor_model_variant; + /// Get execution policy given its name. static auto get_execution_policy(std::string_view) -> beluga_ros::Amcl::execution_policy_variant; + /// Instantiate particle filter given an initial occupancy grid map and the current parametrization. auto make_particle_filter(const nav_msgs::OccupancyGrid::ConstPtr&) const -> std::unique_ptr; + /// Callback for `dynamic_reconfigure` updates. void config_callback(beluga_amcl::AmclConfig& config, uint32_t level); + /// Callback for occupancy grid map updates. void map_callback(const nav_msgs::OccupancyGrid::ConstPtr& message); + /// Callback for repeated map initialization requests. void map_timer_callback(const ros::TimerEvent& ev); + /// Callback for the map update service. bool set_map_callback(nav_msgs::SetMap::Request& request, nav_msgs::SetMap::Response& response); + /// Particle filter (re)initialization helper method. + /** + * \internal + */ void handle_map_with_default_initial_pose(const nav_msgs::OccupancyGrid::ConstPtr& map); + /// Callback for periodic particle cloud updates. void particle_cloud_timer_callback(const ros::TimerEvent& ev); + /// Callback for laser scan updates. void laser_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan); + /// Callback for pose (re)initialization. void initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& message); + /// Callback for the global relocalization service. bool global_localization_callback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + /// Callback for the no motion update service. bool nomotion_update_callback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + /// Callback for periodic pose saving. void save_pose_timer_callback(const ros::TimerEvent& ev); /// Initialize particles from an estimated pose and covariance. @@ -108,41 +134,71 @@ class AmclNodelet : public nodelet::Nodelet { */ bool initialize_from_map(); + /// Callback for estimated pose covariance diagnostics. void update_covariance_diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status); + /// Mutex for particle filter access. mutable std::mutex mutex_; + /// Timer for periodic particle cloud updates. ros::Timer particle_cloud_timer_; + /// Particle cloud publisher. ros::Publisher particle_cloud_pub_; + /// Estimated pose publisher. ros::Publisher pose_pub_; + /// Timer for pose saving. ros::Timer save_pose_timer_; + /// Pose (re)initialization subscriber. ros::Subscriber initial_pose_sub_; + /// Occupancy grid map updates subscriber ros::Subscriber map_sub_; + + /// Timer for repeated map initialization requests. ros::Timer map_timer_; + /// Map update service server. ros::ServiceServer set_map_server_; + /// Map initialization service client. ros::ServiceClient get_map_client_; + /// Global relocalization service server. ros::ServiceServer global_localization_server_; + /// No motion update service server. ros::ServiceServer nomotion_update_server_; + /// Flag set on first configuration. bool configured_{false}; + /// Current `beluga_amcl` configuration. beluga_amcl::AmclConfig config_; + /// Default `beluga_amcl` configuration. beluga_amcl::AmclConfig default_config_; + /// Type alias for a `dynamic_reconfigure::Server` bound to `beluga_amcl` configuration. using AmclConfigServer = dynamic_reconfigure::Server; + /// `beluga_amcl` configuration server. std::unique_ptr config_server_; + /// Transforms buffer. std::unique_ptr tf_buffer_; + /// Transforms broadcaster. std::unique_ptr tf_broadcaster_; + /// Transforms listener. std::unique_ptr tf_listener_; + /// Diagnostics updater. diagnostic_updater::Updater diagnosic_updater_; + /// Laser scan updates subscriber. message_filters::Subscriber laser_scan_sub_; + /// Transform synchronization filter for laser scan updates. std::unique_ptr> laser_scan_filter_; + /// Connection for laser scan updates filter and callback. message_filters::Connection laser_scan_connection_; + /// Particle filter instance. std::unique_ptr particle_filter_; + /// Last known pose estimate, if any. std::optional> last_known_estimate_; + /// Last known occupancy grid map. nav_msgs::OccupancyGrid::ConstPtr last_known_map_; + /// Whether to broadcast transforms or not. bool enable_tf_broadcast_{false}; }; diff --git a/beluga_example/README.md b/beluga_example/README.md index 1e6996862..4348c7773 100644 --- a/beluga_example/README.md +++ b/beluga_example/README.md @@ -4,7 +4,80 @@ This package contains example launch files that demonstrate the use of Beluga-ba ## Examples -See the [getting started](../GETTING_STARTED.md) tutorial to setup a development container or install the package and dependencies from source. +1. **Run an example application using a ROS bag** (inside development container). + + For ROS 2 distributions, run: + ```bash + cd /ws + source install/setup.bash + ros2 launch beluga_example perfect_odometry.launch.xml + ``` + + For ROS 1 distributions, run: + ```bash + cd /ws + source devel*/setup.bash + roslaunch beluga_example perfect_odometry.launch + ``` + +1. **Run an example application using a simulation and teleop controls** (inside development container). + + For ROS 2 distributions, in two separate terminals run: + ```bash + cd /ws + source install/setup.bash + ros2 launch beluga_example simulation.launch.xml + ``` + ```bash + ros2 run teleop_twist_keyboard teleop_twist_keyboard + ``` + + For ROS 1 distributions, in two separate terminals run: + ```bash + cd /ws + source devel*/setup.bash + roslaunch beluga_example simulation.launch + ``` + ```bash + rosrun teleop_twist_keyboard teleop_twist_keyboard + ``` + +1. **Launch a localization node manually**. + + For ROS 2 distributions, run: + ```bash + ros2 launch beluga_example localization_launch.py use_composition:=True localization_params_file:= localization_map:= + ``` + + For ROS 1 distributions, run: + ```bash + roslaunch beluga_example localization.launch localization_params_file:= localization_map:= + ``` + + The `localization_params_file` argument can be ommited if the [default AMCL parameters](beluga_example/params/default.ros2.yaml) are compatible with the robot. + +1. **Use RViz to visualize the localization output**. + + For ROS 2 distributions, run: + ```bash + rviz2 -d $(ros2 pkg prefix --share beluga_example)/rviz/rviz.ros2.rviz + ``` + + For ROS 1 distributions, run: + ```bash + rviz -d $(rospack find beluga_example)/rviz/rviz.ros.rviz + ``` + + **Quality of Service** + + In ROS 2, when subscribing to the output topics from localization, we recommend the following [QoS](https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Quality-of-Service-Settings.html) settings: + + | Topic | Depth | History | Reliability | Durability | + |------------------|-------|--------------|--------------|-----------------| + | `map` | 5 | Keep last | Reliable | Transient local | + | `particle_cloud` | 5 | Keep last | Best effort | Volatile | + | `pose` | 5 | Keep last | Reliable | Volatile | + - Launch a pre-recorded ROS bag with perfect odometry and Beluga AMCL. ```bash diff --git a/beluga_ros/docs/Doxyfile b/beluga_ros/docs/Doxyfile new file mode 100644 index 000000000..50cf0665f --- /dev/null +++ b/beluga_ros/docs/Doxyfile @@ -0,0 +1,12 @@ +PROJECT_NAME = "Beluga ROS" +INPUT = ../include ./_doxygen/beluga_ros-main.md +EXAMPLE_PATH = ../test +RECURSIVE = YES +USE_MDFILE_AS_MAINPAGE = ./_doxygen/beluga_ros-main.md +CITE_BIB_FILES = ./references.bib +INHERIT_DOCS = YES +GENERATE_HTML = YES +GENERATE_LATEX = NO +WARN_AS_ERROR = YES +SHOW_NAMESPACES = YES +USE_MATHJAX = YES diff --git a/beluga_ros/docs/Makefile b/beluga_ros/docs/Makefile new file mode 100644 index 000000000..a62ead8e9 --- /dev/null +++ b/beluga_ros/docs/Makefile @@ -0,0 +1,31 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +FIND ?= find +RM ?= rm + +html: Makefile + @$(SPHINXBUILD) -M html "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +clean: + @$(RM) -fr "$(BUILDDIR)" `$(FIND) "$(SOURCEDIR)" -name generated -type d` + +.PHONY: clean Makefile diff --git a/beluga_ros/docs/_doxygen/beluga_ros-main.md b/beluga_ros/docs/_doxygen/beluga_ros-main.md new file mode 100644 index 000000000..27ac6d33b --- /dev/null +++ b/beluga_ros/docs/_doxygen/beluga_ros-main.md @@ -0,0 +1,30 @@ +# API Reference + +This is the API reference for Beluga ROS. It provides utilities to aid Beluga integration with ROS and ROS 2. + +### Components + +Explore the library's components: + +#### Generic filters + +Generic MCL implementations ready for ROS integration. + +| | | +|-|-| +| beluga_ros::Amcl | 2D lidar Adaptive MCL algorithm, functionally equivalent to that of [Nav2 AMCL](https://index.ros.org/p/nav2_amcl). | + +#### Data structures + +Thin, Beluga-compatible wrappers for typical ROS data structures (usually messages). + +| | | +|-|-| +| beluga_ros::LaserScan | A beluga::BaseLaserScan subclass that wraps [sensor_msgs/LaserScan](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/LaserScan.html) messages. | +| beluga_ros::OccupancyGrid | A beluga::BaseOccupancyGrid2 subclass that wraps [nav_msgs/OccupancyGrid](https://docs.ros.org/en/noetic/api/nav_msgs/html/msg/OccupancyGrid.html) messages. | + +#### Utilities + +| | | +|-|-| +| [tf2_sophus.hpp](@ref tf2_sophus.hpp) | tf2 message conversion API overloads for Sophus types. | diff --git a/beluga_ros/docs/_images b/beluga_ros/docs/_images new file mode 120000 index 000000000..097050528 --- /dev/null +++ b/beluga_ros/docs/_images @@ -0,0 +1 @@ +../../docs/_images \ No newline at end of file diff --git a/beluga_ros/docs/_static b/beluga_ros/docs/_static new file mode 120000 index 000000000..7fdcaa74f --- /dev/null +++ b/beluga_ros/docs/_static @@ -0,0 +1 @@ +../../docs/_static \ No newline at end of file diff --git a/beluga_ros/docs/conf.py b/beluga_ros/docs/conf.py new file mode 100644 index 000000000..04b1c1460 --- /dev/null +++ b/beluga_ros/docs/conf.py @@ -0,0 +1,84 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Configuration file for the Sphinx documentation builder. +# +# For the full list of built-in configuration values, see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +import subprocess + +# -- Project information ----------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information + +project = "Beluga ROS" +copyright = "2022-2024 Ekumen, Inc." +author = "Ekumen Research" + +# -- General configuration --------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration +source_suffix = { + ".rst": "restructuredtext", + ".md": "markdown", +} +language = "en" + +extensions = [ + "myst_parser", + "sphinx.ext.mathjax", + "sphinx.ext.duration", + "sphinx.ext.graphviz", + "sphinx_copybutton", + "sphinx.ext.inheritance_diagram", + "sphinx_design", + "sphinx_babel.autodox", + "sphinxcontrib.bibtex", +] + +bibtex_bibfiles = ["references.bib"] + +myst_enable_extensions = ["colon_fence"] +myst_heading_anchors = 4 + +autodox_outdir = "_doxygen/generated" +autodox_projects = {"reference": ""} + +exclude_patterns = ["README.md", "_build", "_external/*.md"] +suppress_warnings = ["myst.header"] + +# -- Options for HTML output ------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output + +html_theme = "sphinx_book_theme" +html_theme_options = { + "show_navbar_depth": 1, + "collapse_navbar": True, + "collapse_navigation": True, + "repository_branch": "main", + "repository_url": "https://github.com/Ekumen-OS/beluga", + "use_repository_button": True, + "use_edit_page_button": False, + "home_page_in_toc": True, + "logo": { + "image_light": "_images/logo_with_name_light.png", + "image_dark": "_images/logo_with_name_dark.png", + }, +} +html_last_updated_fmt = subprocess.check_output( + ["git", "log", "--pretty=format:'%ad, %h'", "-n1"] +).decode("utf-8") +html_favicon = "_images/logo_200x200.png" +html_css_files = ["custom.css"] +html_js_files = ["custom.js"] +html_static_path = ["_static"] diff --git a/beluga_ros/docs/index.md b/beluga_ros/docs/index.md new file mode 100644 index 000000000..95d2c16c3 --- /dev/null +++ b/beluga_ros/docs/index.md @@ -0,0 +1,10 @@ +# Overview + +```{toctree} +:hidden: +:maxdepth: 1 + +API Reference <_doxygen/generated/reference/html/index> +``` + +Beluga ROS is a C++17 library to help integrate Beluga with ROS (1) and ROS 2 systems. diff --git a/beluga_ros/docs/references.bib b/beluga_ros/docs/references.bib new file mode 100644 index 000000000..9dd1549d5 --- /dev/null +++ b/beluga_ros/docs/references.bib @@ -0,0 +1,67 @@ +@book{thrun2005probabilistic, + title={Probabilistic Robotics}, + author={Thrun, S. and Burgard, W. and Fox, D.}, + isbn={9780262201629}, + lccn={20050436}, + series={Intelligent Robotics and Autonomous Agents series}, + url={https://books.google.com.ar/books?id=jtSMEAAAQBAJ}, + year={2005}, + publisher={MIT Press} +} + +@book{gentle2009computationalstatistics, + title={Computational Statistics}, + author={Gentle, J. E.}, + isbn={9780387981437}, + series={Statistics and Computing}, + year={2009}, + publisher={Springer}, + doi={10.1007/978-0-387-98144-4} +} + +@article{grisetti2007selectiveresampling, + author={Grisetti, Giorgio and Stachniss, Cyrill and Burgard, Wolfram}, + journal={IEEE Transactions on Robotics}, + title={Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters}, + year={2007}, + volume={23}, + number={1}, + pages={34-46}, + doi={10.1109/TRO.2006.889486} +} + +@article{tiacheng2015resamplingmethods, + author={Li, Tiancheng and Bolic, Miodrag and Djuric, Petar M.}, + journal={IEEE Signal Processing Magazine}, + title={Resampling Methods for Particle Filtering: Classification, implementation, and strategies}, + year={2015}, + volume={32}, + number={3}, + pages={70-86}, + doi={10.1109/MSP.2014.2330626} +} + +@inproceedings{fox2001adaptivekldsampling, + author={Fox, Dieter}, + title={KLD-Sampling: Adaptive Particle Filters}, + year={2001}, + publisher={MIT Press}, + address={Cambridge, MA, USA}, + booktitle={Proceedings of the 14th International Conference on Neural Information Processing Systems: Natural and Synthetic}, + pages={713-720}, + numpages={8}, + location={Vancouver, British Columbia, Canada}, + series = {NIPS'01}, + doi={10.5555/2980539.2980632} +} + +@article{kong1994sequentialimputations, + auhor={Kong, A., Liu, J. S., & Wong, W. H.}, + journal={Journal of the American Statistical Association}, + title={Sequential Imputations and Bayesian Missing Data Problems}, + year={1994}, + volume={89}, + number={425}, + pages={278-288}, + doi={10.1080/01621459.1994.10476469} +} diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 29b8dc8f7..4da22e571 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -25,39 +25,83 @@ #include +/** + * \file + * \brief Generic two-dimensional implementation of the Adaptive Monte Carlo Localization (AMCL) algorithm in 2D. + */ + namespace beluga_ros { /// Struct containing parameters for the Adaptive Monte Carlo Localization (AMCL) implementation. struct AmclParams { + /// \brief Translational movement required from last resample for resampling to happen again. double update_min_d = 0.25; + + /// \brief Rotational movement required from last resample for resampling to happen again. double update_min_a = 0.2; + + /// \brief Number of filter updates required before resampling. std::size_t resample_interval = 1UL; + + /// \brief Whether to enable selective resampling \cite grisetti2007selectiveresampling + /// to help avoid loss of diversity in the particle population. The resampling + /// step will only happen if the effective number of particles + /// (\f$N_{eff} = 1/ {\sum w_i^2}\f$) is lower than half the current number of + /// particles, where \f$w_i\f$ refers to the normalized weight of each particle. bool selective_resampling = false; + + /// \brief Minimum allowed number of particles. std::size_t min_particles = 500UL; + + /// \brief Maximum allowed number of particles. std::size_t max_particles = 2000UL; + + /// \brief Exponential decay rate for the slow average weight filter, used in deciding when to + /// recover from a bad approximation by adding random poses \cite thrun2005probabilistic . double alpha_slow = 0.001; + + /// \brief Exponential decay rate for the fast average weight filter, used in deciding when to + /// recover from a bad approximation by adding random poses \cite thrun2005probabilistic . double alpha_fast = 0.1; + + /// \brief Maximum particle filter population error between the true distribution and the + /// estimated distribution. It is used in KLD resampling \cite fox2001adaptivekldsampling + /// to limit the allowed number of particles to the minimum necessary. double kld_epsilon = 0.05; + + /// \brief Upper standard normal quantile for \f$P\f$, where \f$P\f$ is the probability that the error in + /// the estimated distribution will be less than `kld_epsilon` in KLD resampling \cite fox2001adaptivekldsampling . double kld_z = 3.0; + + /// \brief Spatial resolution along the x-axis to create buckets for KLD resampling. double spatial_resolution_x = 0.5; + + /// \brief Spatial resolution along the y-axis to create buckets for KLD resampling. double spatial_resolution_y = 0.5; + + /// \brief Spatial resolution around the z-axis to create buckets for KLD resampling. double spatial_resolution_theta = 10 * Sophus::Constants::pi() / 180; }; -/// Implementation of the Adaptive Monte Carlo Localization (AMCL) algorithm. +/// Implementation of the 2D Adaptive Monte Carlo Localization (AMCL) algorithm. +/// Generic two-dimensional implementation of the Adaptive Monte Carlo Localization (AMCL) algorithm in 2D. class Amcl { public: + /// Weighted SE(2) state particle type. using particle_type = std::tuple; + /// Motion model variant type for runtime selection support. using motion_model_variant = std::variant< beluga::DifferentialDriveModel, // beluga::OmnidirectionalDriveModel, // beluga::StationaryModel>; + /// Sensor model variant type for runtime selection support. using sensor_model_variant = std::variant< beluga::LikelihoodFieldModel, // beluga::BeamSensorModel>; + /// Execution policy variant type for runtime selection support. using execution_policy_variant = std::variant; /// Constructor. @@ -72,7 +116,7 @@ class Amcl { beluga_ros::OccupancyGrid map, motion_model_variant motion_model, sensor_model_variant sensor_model, - const AmclParams& params = AmclParams{}, + const AmclParams& params = AmclParams(), execution_policy_variant execution_policy = std::execution::seq) : params_{params}, map_distribution_{map}, diff --git a/beluga_ros/include/beluga_ros/beluga_ros.hpp b/beluga_ros/include/beluga_ros/beluga_ros.hpp new file mode 100644 index 000000000..a1cf36171 --- /dev/null +++ b/beluga_ros/include/beluga_ros/beluga_ros.hpp @@ -0,0 +1,33 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ROS_BELUGA_ROS_HPP +#define BELUGA_ROS_BELUGA_ROS_HPP + +/** + * \file + * \brief Includes all the Beluga ROS API. + */ + +/** + * \namespace beluga_ros + * \brief The main Beluga ROS namespace + */ + +#include +#include +#include +#include + +#endif diff --git a/beluga_ros/include/beluga_ros/laser_scan.hpp b/beluga_ros/include/beluga_ros/laser_scan.hpp index 700e73ee8..5b7470853 100644 --- a/beluga_ros/include/beluga_ros/laser_scan.hpp +++ b/beluga_ros/include/beluga_ros/laser_scan.hpp @@ -23,15 +23,32 @@ #include +/** + * \file + * \brief Implementation of `sensor_msgs/LaserScan` wrapper type. + */ + namespace beluga_ros { +/// Thin wrapper type for 2D `sensor_msgs/LaserScan` messages. class LaserScan : public beluga::BaseLaserScan { public: + /// Range type. using Scalar = double; + /// Constructor. + /// + /// \param scan Laser scan message. + /// \param origin Laser scan frame origin in the filter frame. + /// Note it is a transform in 3D because the frame lidars typically + /// report data in is in general not coplanar with the plane on which + /// 2D localization operates. + /// \param max_beams Maximum number of beams to consider. + /// \param min_range Minimum allowed range value (in meters). + /// \param max_range Maximum allowed range value (in meters). explicit LaserScan( beluga_ros::msg::LaserScanConstSharedPtr scan, - Sophus::SE3d origin = Sophus::SE3d{}, + Sophus::SE3d origin = Sophus::SE3d(), std::size_t max_beams = std::numeric_limits::max(), Scalar min_range = std::numeric_limits::min(), Scalar max_range = std::numeric_limits::max()) @@ -43,8 +60,10 @@ class LaserScan : public beluga::BaseLaserScan { assert(scan_ != nullptr); } + /// Get the laser scan frame origin in the filter frame. [[nodiscard]] const auto& origin() const { return origin_; } + /// Get laser scan measurement angles as a range. [[nodiscard]] auto angles() const { return ranges::views::iota(0, static_cast(scan_->ranges.size())) | beluga::views::take_evenly(max_beams_) | ranges::views::transform([this](int i) { @@ -52,13 +71,16 @@ class LaserScan : public beluga::BaseLaserScan { }); } + /// Get laser scan range measurements as a range. [[nodiscard]] auto ranges() const { return scan_->ranges | beluga::views::take_evenly(max_beams_) | ranges::views::transform([](auto value) { return static_cast(value); }); } + /// Get the minimum range measurement. [[nodiscard]] auto min_range() const { return min_range_; } + /// Get the maximum range measurement. [[nodiscard]] auto max_range() const { return max_range_; } private: diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp index b1dfb327e..1d57f834c 100644 --- a/beluga_ros/include/beluga_ros/messages.hpp +++ b/beluga_ros/include/beluga_ros/messages.hpp @@ -29,6 +29,7 @@ #error BELUGA_ROS_VERSION is not defined or invalid #endif +/// Compatibility layer for ROS 1 and ROS 2 messages. namespace beluga_ros::msg { #if BELUGA_ROS_VERSION == 2 diff --git a/beluga_ros/include/beluga_ros/occupancy_grid.hpp b/beluga_ros/include/beluga_ros/occupancy_grid.hpp index ea5c9cec5..668ed59d5 100644 --- a/beluga_ros/include/beluga_ros/occupancy_grid.hpp +++ b/beluga_ros/include/beluga_ros/occupancy_grid.hpp @@ -36,39 +36,63 @@ #error BELUGA_ROS_VERSION is not defined or invalid #endif +/** + * \file + * \brief Implementation of `nav_msgs/OccupancyGrid` wrapper type. + */ + namespace beluga_ros { +/// Thin wrapper type for 2D `nav_msgs/OccupancyGrid` messages. class OccupancyGrid : public beluga::BaseOccupancyGrid2 { public: + /// Traits for occupancy grid value interpretation. + /** + * Assumes a [standard ROS trinary interpretation](https://wiki.ros.org/map_server#Value_Interpretation). + */ struct ValueTraits { - // https://wiki.ros.org/map_server#Value_Interpretation - // Supporting standard trinary interpretation. + /// \brief Free value in the standard ROS trinary interpretation. static constexpr std::int8_t kFreeValue = 0; + /// \brief Unknown value in the standard ROS trinary interpretation. static constexpr std::int8_t kUnknownValue = -1; + /// \brief Occupied value in the standard ROS trinary interpretation. static constexpr std::int8_t kOccupiedValue = 100; + /// Check if the given `value` corresponds to that of a free cell. [[nodiscard]] static bool is_free(std::int8_t value) { return value == kFreeValue; } + /// Check if the given `value` corresponds to that of a cell of unknown occupancy. [[nodiscard]] static bool is_unknown(std::int8_t value) { return value == kUnknownValue; } + /// Check if the given `value` corresponds to that of an occupied cell. [[nodiscard]] static bool is_occupied(std::int8_t value) { return value == kOccupiedValue; } }; + /// Constructor. + /// + /// \param grid Occupancy grid message. explicit OccupancyGrid(beluga_ros::msg::OccupancyGridConstSharedPtr grid) : grid_(std::move(grid)), origin_(make_origin_transform(grid_->info.origin)) {} + /// Get the occupancy grid origin in the occupancy grid frame. [[nodiscard]] const Sophus::SE2d& origin() const { return origin_; } + /// Get the size of the occupancy grid (`width()` times `height()`). [[nodiscard]] std::size_t size() const { return grid_->data.size(); } + /// Get a reference to the underlying data storeage (ie. a row-major array). [[nodiscard]] const auto& data() const { return grid_->data; } + /// Get the width of the occupancy grid. [[nodiscard]] std::size_t width() const { return grid_->info.width; } + /// Get the height of the occupancy grid. [[nodiscard]] std::size_t height() const { return grid_->info.height; } + /// Get the resolution of the occupancy grid discretization, in meters. [[nodiscard]] double resolution() const { return grid_->info.resolution; } + /// Get the traits for occupancy grid value interpretation. [[nodiscard]] static auto value_traits() { return ValueTraits{}; } private: diff --git a/beluga_ros/include/beluga_ros/tf2_sophus.hpp b/beluga_ros/include/beluga_ros/tf2_sophus.hpp index 19ea77b05..ba986a276 100644 --- a/beluga_ros/include/beluga_ros/tf2_sophus.hpp +++ b/beluga_ros/include/beluga_ros/tf2_sophus.hpp @@ -34,6 +34,12 @@ #include #include +/** + * \file + * \brief Message conversion API overloads for `Sophus` types. + */ + +/// `tf2` namespace extension for message conversion overload resolution. namespace tf2 { /// Converts a Sophus SE2 type to a Pose message. @@ -201,7 +207,7 @@ inline void fromMsg(const beluga_ros::msg::Pose& msg, Sophus::SE3& out) /// Converts a Sophus (ie. Eigen) 3x3 covariance matrix to a 6x6 row-major array. /** * \param in A Sophus (ie. Eigen) 3x3 covariance matrix of a 2D pose (x, y, yaw). - * \parma out A row-major array of 36 covariance values of a 3D pose. + * \param out A row-major array of 36 covariance values of a 3D pose. * \return a reference to `out`. */ template