From 4260eda4bed0164bd41ba6eb95fe95d1652a36fa Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 11 Dec 2024 14:37:09 +0100 Subject: [PATCH] Imported upstream version '1.3.0' of 'upstream' --- .github/workflows/build-ros.yml | 12 -- .github/workflows/check-clang-format.yml | 2 +- README.md | 61 ++++---- docs/source/building-maps.rst | 3 + .../concept-slam-configuration-file.rst | 10 +- docs/source/dataset-conversions.rst | 8 +- docs/source/geo-referencing.rst | 19 +-- docs/source/index.rst | 3 +- docs/source/refs.bib | 9 ++ docs/source/ros2api.rst | 148 ++++++++++++++++++ .../source/tutorial-mulran-replay-to-ros2.rst | 88 +++++++++++ docs/source/tutorials.rst | 1 + kitti_metrics_eval/CHANGELOG.rst | 3 + kitti_metrics_eval/package.xml | 2 +- mola/CHANGELOG.rst | 3 + mola/package.xml | 2 +- mola_bridge_ros2/CHANGELOG.rst | 5 + .../include/mola_bridge_ros2/BridgeROS2.h | 2 + mola_bridge_ros2/package.xml | 2 +- mola_bridge_ros2/src/BridgeROS2.cpp | 57 +++++++ mola_demos/CHANGELOG.rst | 6 + .../mola-cli-launchs/mulran_just_replay.yaml | 5 + .../mulran_just_replay_to_ros2.yaml | 7 +- mola_demos/package.xml | 2 +- mola_demos/rviz2/mulran.rviz | 29 ++-- mola_imu_preintegration/CHANGELOG.rst | 3 + mola_imu_preintegration/package.xml | 2 +- mola_input_euroc_dataset/CHANGELOG.rst | 3 + mola_input_euroc_dataset/package.xml | 2 +- mola_input_kitti360_dataset/CHANGELOG.rst | 3 + mola_input_kitti360_dataset/package.xml | 2 +- mola_input_kitti_dataset/CHANGELOG.rst | 3 + mola_input_kitti_dataset/package.xml | 2 +- mola_input_mulran_dataset/CHANGELOG.rst | 5 + .../mola_input_mulran_dataset/MulranDataset.h | 31 +++- mola_input_mulran_dataset/package.xml | 2 +- .../src/MulranDataset.cpp | 108 +++++++++++++ mola_input_paris_luco_dataset/CHANGELOG.rst | 3 + mola_input_paris_luco_dataset/package.xml | 2 +- mola_input_rawlog/CHANGELOG.rst | 3 + mola_input_rawlog/package.xml | 2 +- mola_input_rosbag2/CHANGELOG.rst | 5 + mola_input_rosbag2/package.xml | 2 +- mola_input_rosbag2/src/Rosbag2Dataset.cpp | 2 +- mola_kernel/CHANGELOG.rst | 5 + mola_kernel/CMakeLists.txt | 4 +- .../mola_kernel/interfaces/NavStateFilter.h | 35 +++++ mola_kernel/package.xml | 2 +- mola_launcher/CHANGELOG.rst | 3 + mola_launcher/package.xml | 2 +- .../robin-map/.github/workflows/ci.yml | 4 +- mola_metric_maps/3rdparty/robin-map/README.md | 2 +- .../3rdparty/robin-map/doxygen.conf | 2 +- .../include/tsl/robin_growth_policy.h | 5 +- .../robin-map/include/tsl/robin_hash.h | 64 +------- .../3rdparty/robin-map/tests/CMakeLists.txt | 4 +- mola_metric_maps/CHANGELOG.rst | 6 + .../include/mola_metric_maps/NDT.h | 9 ++ mola_metric_maps/package.xml | 2 +- mola_metric_maps/src/NDT.cpp | 105 +++++++++++-- mola_msgs/CHANGELOG.rst | 3 + mola_msgs/package.xml | 2 +- mola_navstate_fg/CHANGELOG.rst | 5 + mola_navstate_fg/CMakeLists.txt | 9 ++ mola_navstate_fg/apps/mola-navstate-cli.cpp | 93 +++++++++++ .../include/mola_navstate_fg/NavStateFG.h | 3 + mola_navstate_fg/package.xml | 2 +- mola_navstate_fg/src/NavStateFG.cpp | 8 + .../tests/mola-navstate-fg-params.yaml | 10 ++ mola_navstate_fuse/CHANGELOG.rst | 5 + .../include/mola_navstate_fuse/NavStateFuse.h | 3 + mola_navstate_fuse/package.xml | 2 +- mola_navstate_fuse/src/NavStateFuse.cpp | 6 + mola_pose_list/CHANGELOG.rst | 3 + mola_pose_list/package.xml | 2 +- mola_relocalization/CHANGELOG.rst | 3 + mola_relocalization/package.xml | 2 +- mola_traj_tools/CHANGELOG.rst | 3 + mola_traj_tools/package.xml | 2 +- mola_viz/CHANGELOG.rst | 5 + mola_viz/package.xml | 2 +- mola_viz/src/MolaViz.cpp | 67 ++++++++ mola_yaml/CHANGELOG.rst | 3 + mola_yaml/package.xml | 2 +- .../clang_git_format/config.py | 2 +- scripts/generate_statuses_table.py | 4 - 86 files changed, 992 insertions(+), 187 deletions(-) create mode 100644 docs/source/ros2api.rst create mode 100644 docs/source/tutorial-mulran-replay-to-ros2.rst create mode 100644 mola_navstate_fg/apps/mola-navstate-cli.cpp create mode 100644 mola_navstate_fg/tests/mola-navstate-fg-params.yaml diff --git a/.github/workflows/build-ros.yml b/.github/workflows/build-ros.yml index 6b2aaf6c93..41b58fd70d 100644 --- a/.github/workflows/build-ros.yml +++ b/.github/workflows/build-ros.yml @@ -10,13 +10,6 @@ jobs: DEBIAN_FRONTEND: noninteractive strategy: matrix: - # Github Actions requires a single row to be added to the build matrix. - # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. -# ros_distribution: -# - humble -# - iron -# - rolling - # Define the Docker image(s) associated with each ROS distribution. # The include syntax allows additional variables to be defined, like # docker_image in this case. See documentation: @@ -31,11 +24,6 @@ jobs: ros_distribution: humble ros_version: 2 - # Iron Irwini (May 2023 - November 2024) - - docker_image: ubuntu:jammy - ros_distribution: iron - ros_version: 2 - # Jazzy (2024 - ??) - docker_image: ubuntu:noble ros_distribution: jazzy diff --git a/.github/workflows/check-clang-format.yml b/.github/workflows/check-clang-format.yml index 8fe73ab84a..42b58b8d38 100644 --- a/.github/workflows/check-clang-format.yml +++ b/.github/workflows/check-clang-format.yml @@ -31,7 +31,7 @@ jobs: - name: Install Dependencies run: | - sudo apt install clang-format-11 -yq + sudo apt install clang-format-14 -yq pip3 install --user -r .github/python_clang_format_reqs.txt - name: Check code style diff --git a/README.md b/README.md index dc18a5cc21..de45f37d28 100644 --- a/README.md +++ b/README.md @@ -5,11 +5,14 @@ | Distro | Build dev | Release | | --- | --- | --- | | ROS 2 Humble (u22.04) | [![Build Status](https://build.ros2.org/job/Hdev__mola__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__mola__ubuntu_jammy_amd64/) | [![Version](https://img.shields.io/ros/v/humble/mola)](https://index.ros.org/search/?term=mola) | -| ROS 2 Iron (u22.04) | [![Build Status](https://build.ros2.org/job/Idev__mola__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Idev__mola__ubuntu_jammy_amd64/) | [![Version](https://img.shields.io/ros/v/iron/mola)](https://index.ros.org/search/?term=mola) | | ROS 2 Jazzy (u24.04) | [![Build Status](https://build.ros2.org/job/Jdev__mola__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Jdev__mola__ubuntu_noble_amd64/) | [![Version](https://img.shields.io/ros/v/jazzy/mola)](https://index.ros.org/search/?term=mola) | | ROS 2 Rolling (u24.04) | [![Build Status](https://build.ros2.org/job/Rdev__mola__ubuntu_noble_amd64/badge/icon)](https://build.ros2.org/job/Rdev__mola__ubuntu_noble_amd64/) | [![Version](https://img.shields.io/ros/v/rolling/mola)](https://index.ros.org/search/?term=mola) | -# mola +| EOL Distro | Last Release | +| --- | --- +| ROS 2 Iron (u22.04) | [![Version](https://img.shields.io/ros/v/iron/mola)](https://index.ros.org/search/?term=mola) | + +# MOLA A Modular Optimization framework for Localization and mApping (MOLA). This repository contains the base open-sourced packages for the MOLA project. Other packages exist under the [MOLAorg](https://github.com/MOLAorg/) organization. @@ -18,40 +21,40 @@ Refer to the [official documentation](https://docs.mola-slam.org/latest/) for bu ![kitti_demo_06](https://github.com/MOLAorg/mola/assets/5497818/76cf07e2-ab20-41a3-abc6-df5c5ba74b97) ## ROSCon Talk introducing MOLA +Video ([Slides](https://docs.google.com/presentation/d/1Zg1saZCxlE49jZ4SxPBLaRh7WUYGmfC0fiKUunJg-dA/edit?usp=drivesdk)): -[Slides](https://docs.google.com/presentation/d/1Zg1saZCxlE49jZ4SxPBLaRh7WUYGmfC0fiKUunJg-dA/edit?usp=drivesdk) +[![MOLA ROSCon talk](https://img.youtube.com/vi/FkDnbfVyN2M/0.jpg)](https://www.youtube.com/watch?v=FkDnbfVyN2M) -Video: TBD ## Individual package build status Note: Rows within each cell are for ``amd64`` and ``arm64`` architectures. -| Package | ROS 2 Humble
BinBuild | ROS 2 Iron
BinBuild | ROS 2 Jazzy
BinBuild | ROS 2 Rolling
BinBuild | -| --- | --- | --- | --- | --- | -| kitti_metrics_eval | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__kitti_metrics_eval__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__kitti_metrics_eval__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__kitti_metrics_eval__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__kitti_metrics_eval__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__kitti_metrics_eval__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__kitti_metrics_eval__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__kitti_metrics_eval__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__kitti_metrics_eval__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/) | -| mola | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/) | -| mola_bridge_ros2 | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_bridge_ros2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_bridge_ros2__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_bridge_ros2__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_bridge_ros2__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_bridge_ros2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_bridge_ros2__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_bridge_ros2__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_bridge_ros2__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/) | -| mola_demos | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_demos__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_demos__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_demos__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_demos__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_demos__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_demos__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_demos__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_demos__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_demos__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_demos__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_demos__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_demos__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/) | -| mola_imu_preintegration | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_imu_preintegration__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_imu_preintegration__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_imu_preintegration__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_imu_preintegration__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_imu_preintegration__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_imu_preintegration__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_imu_preintegration__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_imu_preintegration__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/) | -| mola_input_euroc_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_euroc_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_euroc_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_euroc_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_euroc_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_input_euroc_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_input_euroc_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_euroc_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_euroc_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/) | -| mola_input_kitti360_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti360_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti360_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti360_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti360_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_input_kitti360_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_input_kitti360_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_kitti360_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_kitti360_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/) | -| mola_input_kitti_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_input_kitti_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_input_kitti_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_kitti_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_kitti_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/) | -| mola_input_mulran_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_mulran_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_mulran_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_mulran_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_mulran_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_input_mulran_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_input_mulran_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_mulran_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_mulran_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/) | -| mola_input_paris_luco_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_paris_luco_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_paris_luco_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_paris_luco_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_paris_luco_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_input_paris_luco_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_input_paris_luco_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_paris_luco_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_paris_luco_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/) | -| mola_input_rawlog | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_rawlog__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_rawlog__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rawlog__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rawlog__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_input_rawlog__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_input_rawlog__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_rawlog__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_rawlog__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/) | -| mola_input_rosbag2 | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_rosbag2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_rosbag2__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rosbag2__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rosbag2__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_input_rosbag2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_input_rosbag2__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_rosbag2__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_input_rosbag2__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/) | -| mola_kernel | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_kernel__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_kernel__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_kernel__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_kernel__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_kernel__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_kernel__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_kernel__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_kernel__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/) | -| mola_launcher | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/) | -| mola_metric_maps | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/) | -| mola_msgs | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/) | -| mola_navstate_fg | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/) | -| mola_navstate_fuse | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/) | -| mola_pose_list | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/) | -| mola_relocalization | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/) | -| mola_traj_tools | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/) | -| mola_viz | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_viz__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_viz__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_viz__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_viz__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_viz__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_viz__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_viz__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_viz__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_viz__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_viz__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_viz__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_viz__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/) | -| mola_yaml | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_yaml__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_yaml__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_yaml__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_yaml__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Ibin_uJ64__mola_yaml__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__mola_yaml__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_yaml__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__mola_yaml__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/) | +| Package | ROS 2 Humble
BinBuild | ROS 2 Jazzy
BinBuild | ROS 2 Rolling
BinBuild | +| --- | --- | --- | --- | +| kitti_metrics_eval | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__kitti_metrics_eval__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__kitti_metrics_eval__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__kitti_metrics_eval__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__kitti_metrics_eval__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__kitti_metrics_eval__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__kitti_metrics_eval__ubuntu_noble_arm64__binary/) | +| mola | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola__ubuntu_noble_arm64__binary/) | +| mola_bridge_ros2 | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_bridge_ros2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_bridge_ros2__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_bridge_ros2__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_bridge_ros2__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_bridge_ros2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_bridge_ros2__ubuntu_noble_arm64__binary/) | +| mola_demos | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_demos__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_demos__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_demos__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_demos__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_demos__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_demos__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_demos__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_demos__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_demos__ubuntu_noble_arm64__binary/) | +| mola_imu_preintegration | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_imu_preintegration__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_imu_preintegration__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_imu_preintegration__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_imu_preintegration__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_imu_preintegration__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_imu_preintegration__ubuntu_noble_arm64__binary/) | +| mola_input_euroc_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_euroc_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_euroc_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_euroc_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_euroc_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_euroc_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_euroc_dataset__ubuntu_noble_arm64__binary/) | +| mola_input_kitti360_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti360_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti360_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti360_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti360_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti360_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti360_dataset__ubuntu_noble_arm64__binary/) | +| mola_input_kitti_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_kitti_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_kitti_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_kitti_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_kitti_dataset__ubuntu_noble_arm64__binary/) | +| mola_input_mulran_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_mulran_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_mulran_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_mulran_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_mulran_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_mulran_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_mulran_dataset__ubuntu_noble_arm64__binary/) | +| mola_input_paris_luco_dataset | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_paris_luco_dataset__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_paris_luco_dataset__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_paris_luco_dataset__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_paris_luco_dataset__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_paris_luco_dataset__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_paris_luco_dataset__ubuntu_noble_arm64__binary/) | +| mola_input_rawlog | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_rawlog__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_rawlog__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rawlog__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rawlog__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_rawlog__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rawlog__ubuntu_noble_arm64__binary/) | +| mola_input_rosbag2 | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_input_rosbag2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_input_rosbag2__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rosbag2__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_input_rosbag2__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_input_rosbag2__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_input_rosbag2__ubuntu_noble_arm64__binary/) | +| mola_kernel | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_kernel__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_kernel__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_kernel__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_kernel__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_kernel__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_kernel__ubuntu_noble_arm64__binary/) | +| mola_launcher | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/) | +| mola_metric_maps | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/) | +| mola_msgs | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/) | +| mola_navstate_fg | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/) | +| mola_navstate_fuse | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/) | +| mola_pose_list | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/) | +| mola_relocalization | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/) | +| mola_traj_tools | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/) | +| mola_viz | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_viz__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_viz__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_viz__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_viz__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_viz__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_viz__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_viz__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_viz__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_viz__ubuntu_noble_arm64__binary/) | +| mola_yaml | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_yaml__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_yaml__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_yaml__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_yaml__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_yaml__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_yaml__ubuntu_noble_arm64__binary/) | ## Citations diff --git a/docs/source/building-maps.rst b/docs/source/building-maps.rst index 61521b9532..fb50d10016 100644 --- a/docs/source/building-maps.rst +++ b/docs/source/building-maps.rst @@ -285,6 +285,9 @@ Afterwards, visualizing :ref:`metric map files ` (``*.mm``) can | + +.. _tutorial-pub-map-server-to-ros: + 5. Publish the map to ROS 2 ---------------------------------------- diff --git a/docs/source/concept-slam-configuration-file.rst b/docs/source/concept-slam-configuration-file.rst index 4efba0291f..fab8662621 100644 --- a/docs/source/concept-slam-configuration-file.rst +++ b/docs/source/concept-slam-configuration-file.rst @@ -52,7 +52,7 @@ in either absolute path, or relative to the current YAML file being parsed. - type: CLASS_NAME1 # mandatory #... params: # Other module-specific parameters - $include{$(mola-dir module-name)/params/xxxx.yaml} + $include{$(ros2 pkg prefix mola_lidar_odometry)/share/xxx/xxxx.yaml} config: $include{params/xxxx.yaml} @@ -69,3 +69,11 @@ Replace output of external process: ``$(cmd)`` The pattern ``$(cmd arg1 arg2...)`` is replaced by the console output of running the given command with the given arguments. See for example its usage together with ``$include{xxx}`` above. + +Mathematical formulas : ``$f{xxx}`` +----------------------------------------------- +The pattern ``$f{expr}`` can be used to indicate mathematical expressions. +The `ExprTk library `_ is used +(via `mrpt-expr `_), +so refer to their documentation for built-in functions and syntax. +User-defined variables can be used, depending on the context of the YAML file being parsed. diff --git a/docs/source/dataset-conversions.rst b/docs/source/dataset-conversions.rst index 40ed4e62b5..f28267323b 100644 --- a/docs/source/dataset-conversions.rst +++ b/docs/source/dataset-conversions.rst @@ -247,7 +247,7 @@ and launch it using `mola-cli`. See existing ROS launch examples under `ros2-launch (mola_demos package) `_, with corresponding MOLA cli launch files in the `mola-cli-launchs `_ directory. -.. dropdown:: Example: play back a KITTI dataset sequence to ROS 2 +.. dropdown:: Example #1: play back a KITTI dataset sequence to ROS 2 :open: .. code-block:: bash @@ -258,3 +258,9 @@ with corresponding MOLA cli launch files in the `mola-cli-launchs `. + diff --git a/docs/source/geo-referencing.rst b/docs/source/geo-referencing.rst index b8c0df2e0c..53a92b38a9 100644 --- a/docs/source/geo-referencing.rst +++ b/docs/source/geo-referencing.rst @@ -31,24 +31,7 @@ which extends the standard `REP-105 `_ w :width: 500 :align: center -These are the existing frames: - -- ``base_link``: The robot reference frame. For ground vehicles, normally placed at the - center of the rear axle. -- ``odom``: The arbitrary origin for odometry measurements. -- ``map``: The origin of the reference metric map used for localization. -- ``enu``: For geo-referenced maps, the North (``y`` axis), East (``x`` axis), Up (``z`` axis) frame for which - we have reference geodetic coordinates (latitude and longitude). Different maps built in the same zone - will surely have different ``enu`` frames, since it is defined by collected GNSS measurements. -- ``utm``: The origin of the `UTM zone `_ - in which ``enu`` falls. Unlike ``enu``, it is **independent** of the trajectory followed while building the map. - -And this is who is responsible of publishing each transformation: - -- ``odom → base_link``: Odometry module. High-frequency, accurate in the short term, but drifts in the long term. -- ``map → odom``: :ref:`Localization ` module, which corrects the odometry drift. -- ``enu → {map, utm}``: Published by ``mrpt_map_server`` (`github `_), - if fed with a geo-referenced metric map (``.mm``) file. +The existing frames are :ref:`explained here `. | diff --git a/docs/source/index.rst b/docs/source/index.rst index 6641f5899c..c3f3bbb5f1 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -16,6 +16,7 @@ MOLA localization geo-referencing use-cases + ros2api solutions .. toctree:: @@ -43,7 +44,7 @@ MOLA .. toctree:: :maxdepth: 2 :hidden: - :caption: Details + :caption: Learn more mola_architecture tutorials diff --git a/docs/source/refs.bib b/docs/source/refs.bib index da0c4f7d6e..a82c77f633 100644 --- a/docs/source/refs.bib +++ b/docs/source/refs.bib @@ -113,4 +113,13 @@ @article{magnusson2007scan pages={803--827}, year={2007}, publisher={Wiley Online Library} +} + +@INPROCEEDINGS{gskim-2020-mulran, + TITLE={MulRan: Multimodal Range Dataset for Urban Place Recognition}, + AUTHOR={Giseop Kim and Yeong Sang Park and Younghun Cho and Jinyong Jeong and Ayoung Kim}, + BOOKTITLE = { Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) }, + YEAR = { 2020 }, + MONTH = { May }, + ADDRESS = { Paris } } \ No newline at end of file diff --git a/docs/source/ros2api.rst b/docs/source/ros2api.rst new file mode 100644 index 0000000000..41ce2dc7f7 --- /dev/null +++ b/docs/source/ros2api.rst @@ -0,0 +1,148 @@ +.. _mola_ros2api: + +====================== +ROS 2 API +====================== +This page reflects the topics and services that a MOLA system will expose when running a SLAM +or LiDAR-odometry module. At present, this applies to: + +- Any MOLA system including the :ref:`BridgeROS2 ` module: + This module acts as a wrapper of ``mola-kernel`` virtual interfaces implemented in other + MOLA modules and the ROS 2 system. +- :ref:`mola_lidar_odometry` + +.. note:: + + It is recommended to start with the tutorial on how to :ref:`build a map `, + then check out :ref:`how to launch MOLA-LO for ROS 2 `. + +.. image:: https://mrpt.github.io/imgs/mola-lo-ros2-launch-demo-kitti.png + + + +____________________________________________ + +.. contents:: Table of Contents + :depth: 1 + :local: + :backlinks: none + +____________________________________________ + +| + +1. Map loading / saving +-------------------------------------- +During a live SLAM run, ``BridgeROS2`` will look for modules implementing +:ref:`MapServer ` and will expose +these **ROS 2 services** to load or save the current map: + +* ``/map_load``: See ROS docs for `mola_msgs/MapLoad `_ + +* ``/map_save``: See ROS docs for `mola_msgs/MapSave `_ + +.. dropdown:: Example ROS 2 cli service calls + + To save the current map: + + .. code-block:: bash + + ros2 service call /map_save mola_msgs/srv/MapSave "map_path: '/tmp/my_map_file_prefix'" + + To load a map from disk: + + .. code-block:: bash + + ros2 service call /map_load mola_msgs/srv/MapLoad "map_path: '/tmp/my_map_file_prefix'" + +Note that filename **extension** should not be given, since each service implementation +may add a different extension, or even save several files that should all, together, be +later on loaded as one to load the map again. + +Alternatively, you can enable saving the map when mapping is ended by checking +the corresponding checkbox in the +:ref:`MOLA-LO GUI ` (block "6" below): + +.. image:: imgs/gui_parts.png + + +| + +---- + +2. Re-localization +-------------------------------------- +Write me! + +| + +---- + +.. _mola_ros2_tf_frames: + +3. ``/tf`` frames +-------------------------------------- +These frames of reference exist when using MOLA :ref:`geo-referenced ` maps: + +.. figure:: https://mrpt.github.io/imgs/mola_mrpt_ros_geo_referenced_utm_frames.png + :width: 500 + :align: center + +.. note:: + + For non geo-referenced maps, the meaning of all frames are the same but ``utm`` and ``enu`` + will not be present. + +These are the existing frames: + +- ``base_link``: The robot reference frame. For ground vehicles, normally placed at the + center of the rear axle. +- ``odom``: The arbitrary origin for odometry measurements. +- ``map``: The origin of the reference metric map used for localization. +- ``enu``: For geo-referenced maps, the North (``y`` axis), East (``x`` axis), Up (``z`` axis) frame for which + we have reference geodetic coordinates (latitude and longitude). Different maps built in the same zone + will surely have different ``enu`` frames, since it is defined by collected GNSS measurements. +- ``utm``: The origin of the `UTM zone `_ + in which ``enu`` falls. Unlike ``enu``, it is **independent** of the trajectory followed while building the map. + +And this is who is responsible of publishing each transformation: + +- ``odom → base_link``: Odometry module. High-frequency, accurate in the short term, but drifts in the long term. +- ``map → odom``: :ref:`Localization ` module, which corrects the odometry drift. +- ``enu → {map, utm}``: Published by ``mrpt_map_server`` (`github `_), + if fed with a geo-referenced metric map (``.mm``) file. + + + +---- + +4. Map publishing +-------------------------------------- +There are two ways of publishing maps to ROS: + +* Using ``mrpt_map_server`` (`github `_): + the recommended way for static, previously-built maps. In this case, one ROS topic + will be published for each map layer, as described in the package documentation. + See also :ref:`this tutorial `. + +* During a live map building process (e.g. MOLA-LO). + +In this latter case, BridgeROS2 will look for modules implementing +:ref:`MapSourceBase ` and will publish +one **topic** named ``/`` for each map layer. +The metric map layer C++ class will determine the ROS topic type to use. + +.. note:: + + Using the default MOLA LiDAR odometry pipeline, only one map topic will + be generated during mapping: + + * Name: ``/lidar_odometry/localmap_points`` + * Type: ``sensor_msgs/PointCloud2`` + +| + + + + + diff --git a/docs/source/tutorial-mulran-replay-to-ros2.rst b/docs/source/tutorial-mulran-replay-to-ros2.rst new file mode 100644 index 0000000000..5ea8b7c778 --- /dev/null +++ b/docs/source/tutorial-mulran-replay-to-ros2.rst @@ -0,0 +1,88 @@ +.. _tutorial-mulran-replay-to-ros2: + +======================================= +Replay MulRan to ROS2 +======================================= + +This tutorial will guide you through the process of using MOLA to publish a sequence of the MulRan dataset (:cite:`gskim-2020-mulran`) +as ROS 2 topics, so they can be visualized in RViz, FoxGlove, or used in your own system. + +.. note:: + + Users can also access to all MulRan dataset observations directly with the C++ API of :ref:`mola::MulranDataset `. + + +.. contents:: + :depth: 1 + :local: + :backlinks: none + +.. raw:: html + +
+ +
+ +| + +MOLA installation +---------------------------------- +This tutorial requires the installation of these MOLA packages: ``mola_demos``, ``mola_input_mulran_dataset``, ``mola_viz``. + +Following the default :ref:`installation instructions ` is enough. + +| + +Dataset preparation +---------------------------------- +Download the MulRan dataset (:cite:`gskim-2020-mulran`) from their `website `_, +and extract the files anywhere in your system such as the files layout is as expected by :ref:`mola::MulranDataset `. + +Remember setting the environment variable ``MULRAN_BASE_DIR`` to the root directory of your dataset, for example: + +.. code-block:: bash + + export MULRAN_BASE_DIR=/home/myuser/mulran/ + +| + +Launching the demo +---------------------------------- + +To launch the demo, just run: + +.. code-block:: bash + + # Run the default KAIST01 sequence: + ros2 launch mola_demos ros-mulran-play.launch.py + + # Or select another sequence: + ros2 launch mola_demos ros-mulran-play.launch.py mulran_sequence:=DCC02 + +| + +Under the hook +---------------------------------- +This demo comprises two main files: + +1) The **MOLA system configuration** file. It defines two MOLA modules: the MulRan input and the ROS2 bridge. + +.. dropdown:: YAML listing + :icon: code-review + + File: `mulran_just_replay_to_ros2.yaml `_ + + .. literalinclude:: ../../mola_demos/mola-cli-launchs/mulran_just_replay_to_ros2.yaml + :language: yaml + +2) The **ROS 2 launch** file. It invokes ``mola-cli`` and ``rviz2``. + +.. dropdown:: Launch file listing + :icon: code-review + + File: `ros-mulran-play.launch.py `_ + + .. literalinclude:: ../../mola_demos/ros2-launchs/ros-mulran-play.launch.py + :language: python diff --git a/docs/source/tutorials.rst b/docs/source/tutorials.rst index 509027ca9b..f744f18823 100644 --- a/docs/source/tutorials.rst +++ b/docs/source/tutorials.rst @@ -21,4 +21,5 @@ tools. tutorial-mola-main-cli-tools tutorial-mola-cmake-commands + tutorial-mulran-replay-to-ros2 diff --git a/kitti_metrics_eval/CHANGELOG.rst b/kitti_metrics_eval/CHANGELOG.rst index abaee921e0..09f06754c8 100644 --- a/kitti_metrics_eval/CHANGELOG.rst +++ b/kitti_metrics_eval/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package kitti_metrics_eval ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/kitti_metrics_eval/package.xml b/kitti_metrics_eval/package.xml index 9c9a836616..f2c2c0a0ed 100644 --- a/kitti_metrics_eval/package.xml +++ b/kitti_metrics_eval/package.xml @@ -5,7 +5,7 @@ --> kitti_metrics_eval - 1.2.1 + 1.3.0 CLI tool to evaluate the KITTI odometry bechmark metrics to trajectory files Jose-Luis Blanco-Claraco diff --git a/mola/CHANGELOG.rst b/mola/CHANGELOG.rst index c516cc3825..6347e41424 100644 --- a/mola/CHANGELOG.rst +++ b/mola/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola/package.xml b/mola/package.xml index c34a96bc87..9b95ea0927 100644 --- a/mola/package.xml +++ b/mola/package.xml @@ -2,7 +2,7 @@ mola - 1.2.1 + 1.3.0 Metapackage with all core open-sourced MOLA packages. Jose-Luis Blanco-Claraco diff --git a/mola_bridge_ros2/CHANGELOG.rst b/mola_bridge_ros2/CHANGELOG.rst index 377d9e243f..79dfa8056c 100644 --- a/mola_bridge_ros2/CHANGELOG.rst +++ b/mola_bridge_ros2/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package mola_bridge_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* Support publishing IMU readings MOLA -> ROS2 +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ * BUGFIX: Prevent potential race condition diff --git a/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h b/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h index f3ad5c0a45..2d27382788 100644 --- a/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h +++ b/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h @@ -22,6 +22,7 @@ // MRPT: #include +#include #include #include @@ -278,6 +279,7 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer void internalOn(const mrpt::obs::CObservationPointCloud& obs); void internalOn(const mrpt::obs::CObservationRobotPose& obs); void internalOn(const mrpt::obs::CObservationGPS& obs); + void internalOn(const mrpt::obs::CObservationIMU& obs); void internalOn( const mrpt::obs::CObservationPointCloud& obs, bool isSensorTopic, diff --git a/mola_bridge_ros2/package.xml b/mola_bridge_ros2/package.xml index 3fe083a3b3..fac7243d6d 100644 --- a/mola_bridge_ros2/package.xml +++ b/mola_bridge_ros2/package.xml @@ -5,7 +5,7 @@ --> mola_bridge_ros2 - 1.2.1 + 1.3.0 Bidirectional bridge ROS2-MOLA Jose-Luis Blanco-Claraco diff --git a/mola_bridge_ros2/src/BridgeROS2.cpp b/mola_bridge_ros2/src/BridgeROS2.cpp index 7d06c90aea..9a0e8e8de0 100644 --- a/mola_bridge_ros2/src/BridgeROS2.cpp +++ b/mola_bridge_ros2/src/BridgeROS2.cpp @@ -518,6 +518,10 @@ void BridgeROS2::onNewObservation(const CObservation::Ptr& o) { return internalOn(*oGPS); } + else if (auto oIMU = std::dynamic_pointer_cast(o); oIMU) + { + return internalOn(*oIMU); + } else { MRPT_LOG_THROTTLE_WARN_FMT( @@ -833,6 +837,59 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationGPS& obs) } } +void BridgeROS2::internalOn(const mrpt::obs::CObservationIMU& obs) +{ + auto lck = mrpt::lockHelper(rosPubsMtx_); + + // Create the publisher the first time an observation arrives: + const bool is_1st_pub = + rosPubs_.pub_sensors.find(obs.sensorLabel) == rosPubs_.pub_sensors.end(); + auto& pub = rosPubs_.pub_sensors[obs.sensorLabel]; + + if (is_1st_pub) + { + // REP-2003: Sensor sources should use SystemDefaultsQoS + // See: https://ros.org/reps/rep-2003.html + pub = rosNode()->create_publisher( + obs.sensorLabel, rclcpp::SystemDefaultsQoS()); + } + lck.unlock(); + + rclcpp::Publisher::SharedPtr pubImu = + std::dynamic_pointer_cast>(pub); + ASSERT_(pubImu); + + const std::string sSensorFrameId = obs.sensorLabel; + + // Send TF: + mrpt::poses::CPose3D sensorPose; + obs.getSensorPose(sensorPose); + + tf2::Transform transform = mrpt::ros2bridge::toROS_tfTransform(sensorPose); + + geometry_msgs::msg::TransformStamped tfStmp; + tfStmp.transform = tf2::toMsg(transform); + tfStmp.child_frame_id = sSensorFrameId; + tfStmp.header.frame_id = params_.base_link_frame; + tfStmp.header.stamp = myNow(obs.timestamp); + tf_bc_->sendTransform(tfStmp); + + // Send observation: + { + obs.load(); + + // Convert observation MRPT -> ROS + std_msgs::msg::Header header; + header.stamp = myNow(obs.timestamp); + header.frame_id = sSensorFrameId; + + sensor_msgs::msg::Imu imu; + mrpt::ros2bridge::toROS(obs, header, imu); + + pubImu->publish(imu); + } +} + void BridgeROS2::doLookForNewMolaSubs() { using namespace std::string_literals; diff --git a/mola_demos/CHANGELOG.rst b/mola_demos/CHANGELOG.rst index 073d1d3b0a..7255df3682 100644 --- a/mola_demos/CHANGELOG.rst +++ b/mola_demos/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mola_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* Update ROS2 demos for Mulran replay +* mola_viz: Show IMU data in the GUI too +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_demos/mola-cli-launchs/mulran_just_replay.yaml b/mola_demos/mola-cli-launchs/mulran_just_replay.yaml index 4e4414af0f..1783acce95 100644 --- a/mola_demos/mola-cli-launchs/mulran_just_replay.yaml +++ b/mola_demos/mola-cli-launchs/mulran_just_replay.yaml @@ -20,6 +20,9 @@ modules: - raw_sensor_label: gps decimation: 1 win_pos: 5 400 400 400 + - raw_sensor_label: imu + decimation: 10 + win_pos: 5 550 400 400 params: base_dir: ${MULRAN_BASE_DIR} sequence: ${MULRAN_SEQ} @@ -27,6 +30,8 @@ modules: start_paused: ${MOLA_DATASET_START_PAUSED|false} publish_lidar: true publish_ground_truth: true + publish_imu: true + publish_gps: true #lidar_to_ground_truth_1to1: false # ===================== diff --git a/mola_demos/mola-cli-launchs/mulran_just_replay_to_ros2.yaml b/mola_demos/mola-cli-launchs/mulran_just_replay_to_ros2.yaml index df325400e0..9e7ccd2411 100644 --- a/mola_demos/mola-cli-launchs/mulran_just_replay_to_ros2.yaml +++ b/mola_demos/mola-cli-launchs/mulran_just_replay_to_ros2.yaml @@ -20,6 +20,9 @@ modules: - raw_sensor_label: gps decimation: 1 win_pos: 5 400 400 400 + - raw_sensor_label: imu + decimation: 10 + win_pos: 5 550 400 400 params: base_dir: ${MULRAN_BASE_DIR} sequence: ${MULRAN_SEQ} @@ -27,6 +30,8 @@ modules: start_paused: ${MOLA_DATASET_START_PAUSED|false} publish_lidar: true publish_ground_truth: true + publish_imu: true + publish_gps: true #lidar_to_ground_truth_1to1: false # MOLA -> ROS 2 bridge ===================== @@ -40,7 +45,7 @@ modules: # it will be forwarded to /tf as a transformation between: # `reference_frame` -> `base_link_frame` base_link_frame: base_link - reference_frame: odom + reference_frame: map # If true, an nav_msgs/Odometry message will be also published # diff --git a/mola_demos/package.xml b/mola_demos/package.xml index f06444e523..ebc75bc568 100644 --- a/mola_demos/package.xml +++ b/mola_demos/package.xml @@ -5,7 +5,7 @@ --> mola_demos - 1.2.1 + 1.3.0 Demo and example launch files for MOLA Jose-Luis Blanco-Claraco diff --git a/mola_demos/rviz2/mulran.rviz b/mola_demos/rviz2/mulran.rviz index f43b6318cb..bba492a3b0 100644 --- a/mola_demos/rviz2/mulran.rviz +++ b/mola_demos/rviz2/mulran.rviz @@ -55,11 +55,9 @@ Visualization Manager: Value: true gps: Value: true - lidar: - Value: true - map: + imu: Value: true - odom: + lidar: Value: true Marker Scale: 1 Name: TF @@ -70,10 +68,9 @@ Visualization Manager: base_link: gps: {} - lidar: + imu: {} - map: - odom: + lidar: {} Update Interval: 0 Value: true @@ -235,10 +232,24 @@ Visualization Manager: Reliability Policy: Reliable Value: /pf_pose Value: true + - Alpha: 1 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: true + History Length: 1 + Name: Imu + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /imu + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: map + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -318,4 +329,4 @@ Window Geometry: collapsed: true Width: 1850 X: 70 - Y: 534 + Y: 27 diff --git a/mola_imu_preintegration/CHANGELOG.rst b/mola_imu_preintegration/CHANGELOG.rst index 5c85dae562..f4246e5ca1 100644 --- a/mola_imu_preintegration/CHANGELOG.rst +++ b/mola_imu_preintegration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_imu_preintegration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_imu_preintegration/package.xml b/mola_imu_preintegration/package.xml index c620b05193..5c9c8f5197 100644 --- a/mola_imu_preintegration/package.xml +++ b/mola_imu_preintegration/package.xml @@ -5,7 +5,7 @@ --> mola_imu_preintegration - 1.2.1 + 1.3.0 Integrator of IMU angular velocity readings Jose-Luis Blanco-Claraco diff --git a/mola_input_euroc_dataset/CHANGELOG.rst b/mola_input_euroc_dataset/CHANGELOG.rst index 42cf96bb3d..9e76eb08bf 100644 --- a/mola_input_euroc_dataset/CHANGELOG.rst +++ b/mola_input_euroc_dataset/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_input_euroc_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_input_euroc_dataset/package.xml b/mola_input_euroc_dataset/package.xml index e5f37b3e3c..2e7c7626d5 100644 --- a/mola_input_euroc_dataset/package.xml +++ b/mola_input_euroc_dataset/package.xml @@ -5,7 +5,7 @@ --> mola_input_euroc_dataset - 1.2.1 + 1.3.0 Offline RawDataSource from EUROC SLAM datasets Jose-Luis Blanco-Claraco diff --git a/mola_input_kitti360_dataset/CHANGELOG.rst b/mola_input_kitti360_dataset/CHANGELOG.rst index 2175e6bfdb..9a494afbb2 100644 --- a/mola_input_kitti360_dataset/CHANGELOG.rst +++ b/mola_input_kitti360_dataset/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_input_kitti_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_input_kitti360_dataset/package.xml b/mola_input_kitti360_dataset/package.xml index 8dd0eca3e0..54342e1a9f 100644 --- a/mola_input_kitti360_dataset/package.xml +++ b/mola_input_kitti360_dataset/package.xml @@ -5,7 +5,7 @@ --> mola_input_kitti360_dataset - 1.2.1 + 1.3.0 Offline RawDataSource from Kitti-360 datasets Jose-Luis Blanco-Claraco diff --git a/mola_input_kitti_dataset/CHANGELOG.rst b/mola_input_kitti_dataset/CHANGELOG.rst index 9d6a9b30ee..a832adc20f 100644 --- a/mola_input_kitti_dataset/CHANGELOG.rst +++ b/mola_input_kitti_dataset/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_input_kitti_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_input_kitti_dataset/package.xml b/mola_input_kitti_dataset/package.xml index 8c2611a6d6..77faeabee5 100644 --- a/mola_input_kitti_dataset/package.xml +++ b/mola_input_kitti_dataset/package.xml @@ -5,7 +5,7 @@ --> mola_input_kitti_dataset - 1.2.1 + 1.3.0 Offline RawDataSource from Kitti odometry/SLAM datasets Jose-Luis Blanco-Claraco diff --git a/mola_input_mulran_dataset/CHANGELOG.rst b/mola_input_mulran_dataset/CHANGELOG.rst index 9019412478..bb5873137c 100644 --- a/mola_input_mulran_dataset/CHANGELOG.rst +++ b/mola_input_mulran_dataset/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package mola_input_mulran_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* Mulran: Publish IMU too +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h b/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h index f14bb35605..7284614a21 100644 --- a/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h +++ b/mola_input_mulran_dataset/include/mola_input_mulran_dataset/MulranDataset.h @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -41,7 +42,7 @@ namespace mola * HDOP in that struct is computed as reported standard deviation * from the dataset, divided by HDOP_REFERENCE_METERS. * - `radar`: Navtech CIR204-H (Not implemented yet). - * - `imu` : (TBD) (Not implemented yet). + * - `imu` : As mrpt::obs::CObservationIMU. * - Ground truth poses * * Point clouds are published as mrpt::obs::CObservationPointCloud @@ -105,6 +106,7 @@ class MulranDataset : public RawDataSourceBase, */ mrpt::obs::CObservationPointCloud::Ptr getPointCloud(timestep_t step) const; mrpt::obs::CObservationGPS::Ptr getGPS(timestep_t step) const; + mrpt::obs::CObservationIMU::Ptr getIMU(timestep_t step) const; // See docs in base class: size_t datasetSize() const override; @@ -156,9 +158,11 @@ class MulranDataset : public RawDataSourceBase, std::string base_dir_; //!< base dir for "sequences/*". std::string sequence_; //!< "00", "01", ... bool lidar_to_ground_truth_1to1_ = true; - bool publish_lidar_{true}; - bool publish_gps_{true}; - bool publish_ground_truth_{true}; + + bool publish_lidar_ = true; + bool publish_gps_ = true; + bool publish_imu_ = true; + bool publish_ground_truth_ = true; std::optional last_play_wallclock_time_; double last_dataset_time_ = 0; @@ -169,6 +173,7 @@ class MulranDataset : public RawDataSourceBase, Lidar, GNSS, GroundTruth, + IMU, }; struct Entry @@ -179,6 +184,7 @@ class MulranDataset : public RawDataSourceBase, timestep_t lidarIdx = 0; // idx in lstPointCloudFiles_ timestep_t gpsIdx = 0; // row indices in gpsCsvData_ timestep_t gtIdx = 0; // idx in groundTruthTrajectory_ + timestep_t imuIdx = 0; // idx in imuCsvData_ }; std::multimap datasetEntries_; @@ -191,16 +197,29 @@ class MulranDataset : public RawDataSourceBase, read_ahead_lidar_obs_; mrpt::math::CMatrixDouble gpsCsvData_; - - // I found no extrinsics for the GPS sensor in this dataset web (?): + /** I found no extrinsics for the GPS sensor in this dataset web, but it + * *seems* it's the same system than in "Complex Urban Dataset": + * https://sites.google.com/view/complex-urban-dataset/system?authuser=0 + */ mrpt::poses::CPose3D gpsPoseOnVehicle_ = mrpt::poses::CPose3D::Identity(); mrpt::poses::CPose3D ousterPoseOnVehicle_; + mrpt::math::CMatrixDouble imuCsvData_; + + /** It *seems* authors used the same system than in "Complex Urban Dataset": + * https://sites.google.com/view/complex-urban-dataset/system?authuser=0 + * + * So IMU should be aligned XYZ with the vehicle, and 7cm backwards. + */ + mrpt::poses::CPose3D imuPoseOnVehicle_ = + mrpt::poses::CPose3D::FromTranslation(-0.07, .0, .0); + double replay_time_ = .0; std::string seq_dir_; void load_lidar(timestep_t step) const; mrpt::obs::CObservationGPS::Ptr get_gps_by_row_index(size_t row) const; + mrpt::obs::CObservationIMU::Ptr get_imu_by_row_index(size_t row) const; void autoUnloadOldEntries() const; diff --git a/mola_input_mulran_dataset/package.xml b/mola_input_mulran_dataset/package.xml index 05a921af39..7c7a2f7d28 100644 --- a/mola_input_mulran_dataset/package.xml +++ b/mola_input_mulran_dataset/package.xml @@ -5,7 +5,7 @@ --> mola_input_mulran_dataset - 1.2.1 + 1.3.0 Offline RawDataSource from MulRan datasets Jose-Luis Blanco-Claraco diff --git a/mola_input_mulran_dataset/src/MulranDataset.cpp b/mola_input_mulran_dataset/src/MulranDataset.cpp index 07e5d8a512..51b02b40fe 100644 --- a/mola_input_mulran_dataset/src/MulranDataset.cpp +++ b/mola_input_mulran_dataset/src/MulranDataset.cpp @@ -95,6 +95,7 @@ void MulranDataset::initialize_rds(const Yaml& c) YAML_LOAD_MEMBER_OPT(publish_lidar, bool); YAML_LOAD_MEMBER_OPT(publish_gps, bool); + YAML_LOAD_MEMBER_OPT(publish_imu, bool); YAML_LOAD_MEMBER_OPT(publish_ground_truth, bool); // Make list of all existing files and preload everything we may need later @@ -162,6 +163,40 @@ void MulranDataset::initialize_rds(const Yaml& c) MRPT_LOG_INFO_FMT("No GPS file found, expected: '%s'", gpsFile.c_str()); } + // Load IMU data, if found: + const auto imuFile = mrpt::system::pathJoin({seq_dir_, "xsens_imu.csv"}); + if (mrpt::system::fileExists(imuFile)) + { + try + { + imuCsvData_.loadFromTextFile(imuFile); + ASSERT_(imuCsvData_.cols() == 17 || imuCsvData_.cols() == 18); + } + catch (const std::exception& e) + { + MRPT_LOG_ERROR_STREAM( + "Error parsing IMU file: '" << imuFile << "':\n" + << e.what()); + throw; + } + + MRPT_LOG_INFO_STREAM( + "IMU found: " << imuCsvData_.rows() << " entries."); + + // Parse into the unified container: + for (int row = 0; row < imuCsvData_.rows(); row++) + { + const double t = 1e-9 * imuCsvData_(row, 0); + Entry entry = {EntryType::IMU}; + entry.imuIdx = row; + datasetEntries_.emplace(t, entry); + } + } + else + { + MRPT_LOG_INFO_FMT("No IMU file found, expected: '%s'", imuFile.c_str()); + } + // Load ground truth poses, if available: const auto gtFile = mrpt::system::pathJoin({seq_dir_, "global_pose.csv"}); if (!mrpt::system::fileExists(gtFile)) @@ -367,6 +402,15 @@ void MulranDataset::spinOnce() } break; + case EntryType::IMU: + { + if (!publish_imu_) break; + + auto o = get_imu_by_row_index(de.imuIdx); + this->sendObservationsToFrontEnds(o); + } + break; + case EntryType::GroundTruth: { if (!publish_ground_truth_) break; @@ -563,6 +607,19 @@ mrpt::obs::CObservationGPS::Ptr MulranDataset::getGPS(timestep_t step) const return get_gps_by_row_index(it->second.gpsIdx); } +mrpt::obs::CObservationIMU::Ptr MulranDataset::getIMU(timestep_t step) const +{ + ASSERT_(initialized_); + ASSERT_LT_(step, datasetEntries_.size()); + + auto it = datasetEntries_.begin(); + std::advance(it, step); + + if (it->second.type != EntryType::IMU) return {}; + + return get_imu_by_row_index(it->second.imuIdx); +} + mrpt::obs::CObservationGPS::Ptr MulranDataset::get_gps_by_row_index( size_t row) const { @@ -572,6 +629,7 @@ mrpt::obs::CObservationGPS::Ptr MulranDataset::get_gps_by_row_index( auto obs = mrpt::obs::CObservationGPS::Create(); obs->sensorLabel = "gps"; obs->timestamp = mrpt::Clock::fromDouble(1e-9 * gpsCsvData_(row, 0)); + obs->sensorPose = gpsPoseOnVehicle_; // clang-format off // column order: @@ -607,6 +665,52 @@ mrpt::obs::CObservationGPS::Ptr MulranDataset::get_gps_by_row_index( return obs; } +mrpt::obs::CObservationIMU::Ptr MulranDataset::get_imu_by_row_index( + size_t row) const +{ + ASSERT_(initialized_); + ASSERT_LT_(row, static_cast(imuCsvData_.rows())); + + auto obs = mrpt::obs::CObservationIMU::Create(); + obs->sensorLabel = "imu"; + obs->sensorPose = imuPoseOnVehicle_; + obs->timestamp = mrpt::Clock::fromDouble(1e-9 * imuCsvData_(row, 0)); + + // clang-format off + // column order: (length is either 8 or 17) + // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 + // &stamp, &q_x,&q_y,&q_z,&q_w, &x,&y,&z, &g_x,&g_y,&g_z, &a_x,&a_y,&a_z, &m_x,&m_y,&m_z + // clang-format on + + using namespace mrpt::obs; + + obs->set(mrpt::obs::IMU_ORI_QUAT_X, imuCsvData_(row, 1)); + obs->set(mrpt::obs::IMU_ORI_QUAT_Y, imuCsvData_(row, 2)); + obs->set(mrpt::obs::IMU_ORI_QUAT_Z, imuCsvData_(row, 3)); + obs->set(mrpt::obs::IMU_ORI_QUAT_W, imuCsvData_(row, 4)); + + obs->set(mrpt::obs::IMU_X, imuCsvData_(row, 5)); + obs->set(mrpt::obs::IMU_Y, imuCsvData_(row, 6)); + obs->set(mrpt::obs::IMU_Z, imuCsvData_(row, 7)); + + if (imuCsvData_.cols() == 17) + { + obs->set(mrpt::obs::IMU_WX, imuCsvData_(row, 8)); + obs->set(mrpt::obs::IMU_WY, imuCsvData_(row, 9)); + obs->set(mrpt::obs::IMU_WZ, imuCsvData_(row, 10)); + + obs->set(mrpt::obs::IMU_X_ACC, imuCsvData_(row, 11)); + obs->set(mrpt::obs::IMU_Y_ACC, imuCsvData_(row, 12)); + obs->set(mrpt::obs::IMU_Z_ACC, imuCsvData_(row, 13)); + + obs->set(mrpt::obs::IMU_MAG_X, imuCsvData_(row, 14)); + obs->set(mrpt::obs::IMU_MAG_Y, imuCsvData_(row, 15)); + obs->set(mrpt::obs::IMU_MAG_Z, imuCsvData_(row, 16)); + } + + return obs; +} + size_t MulranDataset::datasetSize() const { ASSERT_(initialized_); @@ -631,6 +735,10 @@ mrpt::obs::CSensoryFrame::Ptr MulranDataset::datasetGetObservations( { if (auto o = getGPS(timestep); o) sf->insert(o); } + if (publish_imu_) + { + if (auto o = getIMU(timestep); o) sf->insert(o); + } return sf; } diff --git a/mola_input_paris_luco_dataset/CHANGELOG.rst b/mola_input_paris_luco_dataset/CHANGELOG.rst index 85ce32148c..d6bcaa8971 100644 --- a/mola_input_paris_luco_dataset/CHANGELOG.rst +++ b/mola_input_paris_luco_dataset/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package mola_input_paris_luco_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_input_paris_luco_dataset/package.xml b/mola_input_paris_luco_dataset/package.xml index 854e9827f5..2d653ff4a9 100644 --- a/mola_input_paris_luco_dataset/package.xml +++ b/mola_input_paris_luco_dataset/package.xml @@ -5,7 +5,7 @@ --> mola_input_paris_luco_dataset - 1.2.1 + 1.3.0 Offline RawDataSource from Paris LUCO (CT-ICP) odometry/SLAM datasets Jose-Luis Blanco-Claraco diff --git a/mola_input_rawlog/CHANGELOG.rst b/mola_input_rawlog/CHANGELOG.rst index 0456eac688..cdf2e275d0 100644 --- a/mola_input_rawlog/CHANGELOG.rst +++ b/mola_input_rawlog/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_input_rawlog ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_input_rawlog/package.xml b/mola_input_rawlog/package.xml index 9db7aeae3f..4f0408e3b8 100644 --- a/mola_input_rawlog/package.xml +++ b/mola_input_rawlog/package.xml @@ -5,7 +5,7 @@ --> mola_input_rawlog - 1.2.1 + 1.3.0 Offline RawDataSource from MRPT rawlog datasets Jose-Luis Blanco-Claraco diff --git a/mola_input_rosbag2/CHANGELOG.rst b/mola_input_rosbag2/CHANGELOG.rst index 1ed23e9945..057954b77d 100644 --- a/mola_input_rosbag2/CHANGELOG.rst +++ b/mola_input_rosbag2/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package mola_input_rosbag2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* Fix typo in warning message +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_input_rosbag2/package.xml b/mola_input_rosbag2/package.xml index 53d1629234..11160a9c8d 100644 --- a/mola_input_rosbag2/package.xml +++ b/mola_input_rosbag2/package.xml @@ -5,7 +5,7 @@ --> mola_input_rosbag2 - 1.2.1 + 1.3.0 Offline RawDataSource from rosbag2 datasets Jose-Luis Blanco-Claraco diff --git a/mola_input_rosbag2/src/Rosbag2Dataset.cpp b/mola_input_rosbag2/src/Rosbag2Dataset.cpp index 99e0f92cb3..49bc2c5e29 100644 --- a/mola_input_rosbag2/src/Rosbag2Dataset.cpp +++ b/mola_input_rosbag2/src/Rosbag2Dataset.cpp @@ -981,7 +981,7 @@ Rosbag2Dataset::Obs Rosbag2Dataset::catchExceptions( { MRPT_LOG_ERROR_STREAM( "Exception while processing topic message (ignore if the error " - "stops later one, e.g. missing /tf):\n" + "stops later on, e.g. missing /tf):\n" << e.what()); return {}; } diff --git a/mola_kernel/CHANGELOG.rst b/mola_kernel/CHANGELOG.rst index ea6181dc38..ab168e033f 100644 --- a/mola_kernel/CHANGELOG.rst +++ b/mola_kernel/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mola_kernel ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* NavStateFilter interface: add API for merging GNSS observations +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_kernel/CMakeLists.txt b/mola_kernel/CMakeLists.txt index ef54779052..2d5c832f25 100644 --- a/mola_kernel/CMakeLists.txt +++ b/mola_kernel/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(mola_common REQUIRED) find_package(mola_yaml REQUIRED) # find dependencies: -find_package(MRPT 2.1.0 REQUIRED COMPONENTS gui obs maps) +find_package(MRPT 2.1.0 REQUIRED COMPONENTS gui obs maps topography) # define lib: set(LIB_SRCS @@ -99,11 +99,13 @@ mola_add_library( PUBLIC_LINK_LIBRARIES mrpt::obs mrpt::gui + mrpt::topography mola::mola_yaml # PRIVATE_LINK_LIBRARIES CMAKE_DEPENDENCIES mrpt-obs mrpt-gui + mrpt-topography mola_yaml ) diff --git a/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h b/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h index 9c4e83f5ed..0b76897429 100644 --- a/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h +++ b/mola_kernel/include/mola_kernel/interfaces/NavStateFilter.h @@ -15,8 +15,10 @@ #include #include #include +#include #include #include +#include #pragma once @@ -85,6 +87,9 @@ class NavStateFilter : public mrpt::system::COutputLogger /** Integrates new IMU observations into the estimator */ virtual void fuse_imu(const mrpt::obs::CObservationIMU& imu) = 0; + /** Integrates new GNSS observations into the estimator */ + virtual void fuse_gnss(const mrpt::obs::CObservationGPS& gps) = 0; + /** Integrates new twist estimation (in the odom frame) */ virtual void fuse_twist( const mrpt::Clock::time_point& timestamp, @@ -99,6 +104,36 @@ class NavStateFilter : public mrpt::system::COutputLogger virtual std::optional estimated_navstate( const mrpt::Clock::time_point& timestamp, const std::string& frame_id) = 0; + + /** Must be invoked with the mp2p_icp metric map geo-referencing information + * of the map in order to have GNSS observations correctly fused. + */ + void set_georeferencing_params( + /** The geodetic coordinates (on WGS-84) of the metric map ENU frame of + * reference. */ + mrpt::topography::TGeodeticCoords geo_coord, + /** The SE(3) transformation from the ENU (earth-north-up) frame + * to the metric map local frame of reference. + * If this is the identity (default) it means the map is already in + * ENU coordinates (i.e. +X is East, +Y is North, +Z is up) and + * the point (0,0,0) is the one having the geodetic coordinates + * geo_coord + */ + mrpt::poses::CPose3DPDFGaussian T_enu_to_map) + { + auto& g = geoRefParams_.emplace(); + + g.geo_coord = geo_coord; + g.T_enu_to_map = T_enu_to_map; + } + + protected: + struct GeoReferenceParams + { + mrpt::topography::TGeodeticCoords geo_coord; + mrpt::poses::CPose3DPDFGaussian T_enu_to_map; + }; + std::optional geoRefParams_; }; } // namespace mola diff --git a/mola_kernel/package.xml b/mola_kernel/package.xml index 328715975a..2ab82e697b 100644 --- a/mola_kernel/package.xml +++ b/mola_kernel/package.xml @@ -5,7 +5,7 @@ --> mola_kernel - 1.2.1 + 1.3.0 Fundamental C++ virtual interfaces and data types for the rest of MOLA modules Jose-Luis Blanco-Claraco diff --git a/mola_launcher/CHANGELOG.rst b/mola_launcher/CHANGELOG.rst index 7a2660cf5c..9f99b4823f 100644 --- a/mola_launcher/CHANGELOG.rst +++ b/mola_launcher/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_launcher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_launcher/package.xml b/mola_launcher/package.xml index c2da718068..4be2380b30 100644 --- a/mola_launcher/package.xml +++ b/mola_launcher/package.xml @@ -5,7 +5,7 @@ --> mola_launcher - 1.2.1 + 1.3.0 Launcher app for MOLA systems Jose-Luis Blanco-Claraco diff --git a/mola_metric_maps/3rdparty/robin-map/.github/workflows/ci.yml b/mola_metric_maps/3rdparty/robin-map/.github/workflows/ci.yml index 0acd37734a..96b698a662 100644 --- a/mola_metric_maps/3rdparty/robin-map/.github/workflows/ci.yml +++ b/mola_metric_maps/3rdparty/robin-map/.github/workflows/ci.yml @@ -29,13 +29,13 @@ jobs: } - { name: macos-x64-gcc, - os: macos-latest, + os: macos-13, cxx: g++, cmake-build-type: Release } - { name: macos-x64-clang, - os: macos-latest, + os: macos-13, cxx: clang++, cmake-build-type: Release } diff --git a/mola_metric_maps/3rdparty/robin-map/README.md b/mola_metric_maps/3rdparty/robin-map/README.md index f012a2bbe3..09fc54458c 100644 --- a/mola_metric_maps/3rdparty/robin-map/README.md +++ b/mola_metric_maps/3rdparty/robin-map/README.md @@ -92,7 +92,7 @@ If the project has been installed through `make install`, you can also use `find The library is available in [vcpkg](https://github.com/Microsoft/vcpkg/tree/master/ports/robin-map) and [conan](https://conan.io/center/tsl-robin-map). It's also present in [Debian](https://packages.debian.org/buster/robin-map-dev), [Ubuntu](https://packages.ubuntu.com/disco/robin-map-dev) and [Fedora](https://apps.fedoraproject.org/packages/robin-map-devel) package repositories. -The code should work with any C++11 standard-compliant compiler and has been tested with GCC 4.8.4, Clang 3.5.0 and Visual Studio 2015. +The code should work with any C++17 standard-compliant compiler. To run the tests you will need the Boost Test library and CMake. diff --git a/mola_metric_maps/3rdparty/robin-map/doxygen.conf b/mola_metric_maps/3rdparty/robin-map/doxygen.conf index 276a2a69f7..534fdb5446 100644 --- a/mola_metric_maps/3rdparty/robin-map/doxygen.conf +++ b/mola_metric_maps/3rdparty/robin-map/doxygen.conf @@ -1081,7 +1081,7 @@ CLANG_ASSISTED_PARSING = YES # specified with INPUT and INCLUDE_PATH. # This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. -CLANG_OPTIONS = -std=c++11 +CLANG_OPTIONS = -std=c++17 #--------------------------------------------------------------------------- # Configuration options related to the alphabetical class index diff --git a/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_growth_policy.h b/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_growth_policy.h index b100bfc2b8..2dc9c400c0 100644 --- a/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_growth_policy.h +++ b/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_growth_policy.h @@ -253,7 +253,7 @@ namespace detail { #define TSL_RH_NB_PRIMES 23 #endif -static constexpr const std::array PRIMES = {{ +inline constexpr std::array PRIMES = {{ 1u, 5u, 17u, @@ -319,8 +319,7 @@ static constexpr std::size_t mod(std::size_t hash) { // MOD_PRIME[iprime](hash) returns hash % PRIMES[iprime]. This table allows for // faster modulo as the compiler can optimize the modulo code better with a // constant known at the compilation. -static constexpr const std::array +inline constexpr std::array MOD_PRIME = {{ &mod<0>, &mod<1>, &mod<2>, &mod<3>, &mod<4>, &mod<5>, &mod<6>, &mod<7>, &mod<8>, &mod<9>, &mod<10>, &mod<11>, diff --git a/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_hash.h b/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_hash.h index 03d23a9790..78043e44e5 100644 --- a/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_hash.h +++ b/mola_metric_maps/3rdparty/robin-map/include/tsl/robin_hash.h @@ -66,12 +66,6 @@ template struct is_power_of_two_policy> : std::true_type {}; -// Only available in C++17, we need to be compatible with C++11 -template -const T& clamp(const T& v, const T& lo, const T& hi) { - return std::min(hi, std::max(lo, v)); -} - template static T numeric_cast(U value, const char* error_message = "numeric_cast() failed.") { @@ -254,22 +248,14 @@ class bucket_entry : public bucket_entry_hash { value_type& value() noexcept { tsl_rh_assert(!empty()); -#if defined(__cplusplus) && __cplusplus >= 201703L return *std::launder( reinterpret_cast(std::addressof(m_value))); -#else - return *reinterpret_cast(std::addressof(m_value)); -#endif } const value_type& value() const noexcept { tsl_rh_assert(!empty()); -#if defined(__cplusplus) && __cplusplus >= 201703L return *std::launder( reinterpret_cast(std::addressof(m_value))); -#else - return *reinterpret_cast(std::addressof(m_value)); -#endif } distance_type dist_from_ideal_bucket() const noexcept { @@ -541,7 +527,6 @@ class robin_hash : private Hash, private KeyEqual, private GrowthPolicy { }; public: -#if defined(__cplusplus) && __cplusplus >= 201402L robin_hash(size_type bucket_count, const Hash& hash, const KeyEqual& equal, const Allocator& alloc, float min_load_factor = DEFAULT_MIN_LOAD_FACTOR, @@ -569,47 +554,6 @@ class robin_hash : private Hash, private KeyEqual, private GrowthPolicy { this->min_load_factor(min_load_factor); this->max_load_factor(max_load_factor); } -#else - /** - * C++11 doesn't support the creation of a std::vector with a custom allocator - * and 'count' default-inserted elements. The needed contructor `explicit - * vector(size_type count, const Allocator& alloc = Allocator());` is only - * available in C++14 and later. We thus must resize after using the - * `vector(const Allocator& alloc)` constructor. - * - * We can't use `vector(size_type count, const T& value, const Allocator& - * alloc)` as it requires the value T to be copyable. - */ - robin_hash(size_type bucket_count, const Hash& hash, const KeyEqual& equal, - const Allocator& alloc, - float min_load_factor = DEFAULT_MIN_LOAD_FACTOR, - float max_load_factor = DEFAULT_MAX_LOAD_FACTOR) - : Hash(hash), - KeyEqual(equal), - GrowthPolicy(bucket_count), - m_buckets_data(alloc), - m_buckets(static_empty_bucket_ptr()), - m_bucket_count(bucket_count), - m_nb_elements(0), - m_grow_on_next_insert(false), - m_try_shrink_on_next_insert(false) { - if (bucket_count > max_bucket_count()) { - TSL_RH_THROW_OR_TERMINATE(std::length_error, - "The map exceeds its maximum bucket count."); - } - - if (m_bucket_count > 0) { - m_buckets_data.resize(m_bucket_count); - m_buckets = m_buckets_data.data(); - - tsl_rh_assert(!m_buckets_data.empty()); - m_buckets_data.back().set_as_last_bucket(); - } - - this->min_load_factor(min_load_factor); - this->max_load_factor(max_load_factor); - } -#endif robin_hash(const robin_hash& other) : Hash(other), @@ -1073,13 +1017,13 @@ class robin_hash : private Hash, private KeyEqual, private GrowthPolicy { float max_load_factor() const { return m_max_load_factor; } void min_load_factor(float ml) { - m_min_load_factor = clamp(ml, float(MINIMUM_MIN_LOAD_FACTOR), - float(MAXIMUM_MIN_LOAD_FACTOR)); + m_min_load_factor = std::clamp(ml, float(MINIMUM_MIN_LOAD_FACTOR), + float(MAXIMUM_MIN_LOAD_FACTOR)); } void max_load_factor(float ml) { - m_max_load_factor = clamp(ml, float(MINIMUM_MAX_LOAD_FACTOR), - float(MAXIMUM_MAX_LOAD_FACTOR)); + m_max_load_factor = std::clamp(ml, float(MINIMUM_MAX_LOAD_FACTOR), + float(MAXIMUM_MAX_LOAD_FACTOR)); m_load_threshold = size_type(float(bucket_count()) * m_max_load_factor); tsl_rh_assert(bucket_count() == 0 || m_load_threshold < bucket_count()); } diff --git a/mola_metric_maps/3rdparty/robin-map/tests/CMakeLists.txt b/mola_metric_maps/3rdparty/robin-map/tests/CMakeLists.txt index af9ad7d7db..6d3bbcd09c 100644 --- a/mola_metric_maps/3rdparty/robin-map/tests/CMakeLists.txt +++ b/mola_metric_maps/3rdparty/robin-map/tests/CMakeLists.txt @@ -8,7 +8,7 @@ add_executable(tsl_robin_map_tests "main.cpp" "robin_map_tests.cpp" "robin_set_tests.cpp") -target_compile_features(tsl_robin_map_tests PRIVATE cxx_std_11) +target_compile_features(tsl_robin_map_tests PRIVATE cxx_std_17) if(CMAKE_CXX_COMPILER_ID MATCHES "Clang" OR CMAKE_CXX_COMPILER_ID MATCHES "GNU") target_compile_options(tsl_robin_map_tests PRIVATE -Werror -Wall -Wextra -Wold-style-cast -DTSL_DEBUG -UNDEBUG) @@ -18,7 +18,7 @@ endif() # Boost::unit_test_framework set(Boost_USE_STATIC_LIBS ON) -find_package(Boost 1.54.0 REQUIRED COMPONENTS unit_test_framework) +find_package(Boost REQUIRED COMPONENTS unit_test_framework) target_link_libraries(tsl_robin_map_tests PRIVATE Boost::unit_test_framework) # tsl::robin_map diff --git a/mola_metric_maps/CHANGELOG.rst b/mola_metric_maps/CHANGELOG.rst index 455eeee30f..65f72eb068 100644 --- a/mola_metric_maps/CHANGELOG.rst +++ b/mola_metric_maps/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mola_metric_maps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* NDT maps: more render options (enable colormaps,etc.) +* mola_metric_maps: robin-maps upgraded to latest version +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_metric_maps/include/mola_metric_maps/NDT.h b/mola_metric_maps/include/mola_metric_maps/NDT.h index 54dae071c4..030454a633 100644 --- a/mola_metric_maps/include/mola_metric_maps/NDT.h +++ b/mola_metric_maps/include/mola_metric_maps/NDT.h @@ -482,6 +482,15 @@ class NDT : public mrpt::maps::CMetricMap, mrpt::img::TColorf planes_color{1.0f, .0f, 1.0f}; bool normals_visible = true; mrpt::img::TColorf normals_color{1.0f, 0.0f, 0.0f}; + + /** Colormap to use. Can be set to cmNone for uniform color */ + mrpt::img::TColormap points_colormap = mrpt::img::cmHOT; + mrpt::img::TColormap planes_colormap = mrpt::img::cmHOT; + + /** If colormap!=mrpt::img::cmNONE, use this coordinate + * as color index: 0=x 1=y 2=z + */ + uint8_t recolorizeByCoordinateIndex = 2; }; TRenderOptions renderOptions; diff --git a/mola_metric_maps/package.xml b/mola_metric_maps/package.xml index 5a356c9745..30c26a1282 100644 --- a/mola_metric_maps/package.xml +++ b/mola_metric_maps/package.xml @@ -5,7 +5,7 @@ --> mola_metric_maps - 1.2.1 + 1.3.0 Advanced metric map classes, using the generic `mrpt::maps::CMetricMap` interface, for use in other MOLA odometry and SLAM modules. Jose-Luis Blanco-Claraco diff --git a/mola_metric_maps/src/NDT.cpp b/mola_metric_maps/src/NDT.cpp index 27dae2fe61..cef859637e 100644 --- a/mola_metric_maps/src/NDT.cpp +++ b/mola_metric_maps/src/NDT.cpp @@ -224,27 +224,84 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const MRPT_START if (!genericMapParams.enableSaveAs3DObject) return; + // Calculate histograms / stats: + auto bb = this->boundingBox(); + + // handle planar maps (avoids error in histogram below): + for (int i = 0; i < 3; i++) + if (bb.max[i] - bb.min[i] < 0.1f) bb.max[i] = bb.min[i] + 0.1f; + + // Use a histogram to discard outliers from the colormap extremes: + constexpr size_t nBins = 100; + // for x,y,z + std::array hists = { + mrpt::math::CHistogram(bb.min.x, bb.max.x, nBins), + mrpt::math::CHistogram(bb.min.y, bb.max.y, nBins), + mrpt::math::CHistogram(bb.min.z, bb.max.z, nBins)}; + + size_t nPoints = 0; + + const auto lambdaVisitPointsForHist = + [&hists, &nPoints](const mrpt::math::TPoint3Df& pt) + { + for (int i = 0; i < 3; i++) hists[i].add(pt[i]); + nPoints++; + }; + + this->visitAllPoints(lambdaVisitPointsForHist); + + float recolorMin = .0, recolorMax = 1.f; + if (nPoints) + { + // Analyze the histograms and get confidence intervals: + std::vector coords; + std::vector hits; + + const int idx = renderOptions.recolorizeByCoordinateIndex; + ASSERT_(idx >= 0 && idx < 3); + + constexpr double confidenceInterval = 0.02; + + hists[idx].getHistogramNormalized(coords, hits); + mrpt::math::confidenceIntervalsFromHistogram( + coords, hits, recolorMin, recolorMax, confidenceInterval); + } + const float recolorK = + recolorMax != recolorMin ? 1.0f / (recolorMax - recolorMin) : 1.0f; + + // points: MRPT_TODO("option to hide points already with NDT"); if (renderOptions.points_visible) { - auto obj = mrpt::opengl::CPointCloud::Create(); + auto obj = mrpt::opengl::CPointCloudColoured::Create(); - const auto lambdaVisitPoints = [&obj](const mrpt::math::TPoint3Df& pt) - { obj->insertPoint(pt); }; + const auto lambdaVisitPoints = [&obj](const mrpt::math::TPoint3Df& pt) { + obj->insertPoint({pt.x, pt.y, pt.z, 0, 0, 0}); + }; this->visitAllPoints(lambdaVisitPoints); - obj->setColor(renderOptions.points_color); + if (renderOptions.points_colormap == mrpt::img::cmNONE) + obj->setColor(renderOptions.points_color); + else + { + if (!obj->empty()) + obj->recolorizeByCoordinate( + recolorMin, recolorMax, + renderOptions.recolorizeByCoordinateIndex, + renderOptions.points_colormap); + } + obj->setPointSize(renderOptions.point_size); - obj->enableColorFromZ(false); outObj.insert(obj); } + // planes: if (renderOptions.planes_visible) { auto obj = mrpt::opengl::CSetOfTriangles::Create(); const auto lambdaVisitVoxel = - [&obj, this]( + [&obj, recolorK, recolorMin, this]( [[maybe_unused]] const global_index3d_t& idx, const VoxelData& v) { @@ -264,7 +321,18 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const const auto vy = ndt->eigVectors.at(2).cast() * s; mrpt::opengl::TTriangle t; - t.setColor(renderOptions.planes_color); + + if (renderOptions.planes_colormap == mrpt::img::cmNONE) + t.setColor(renderOptions.planes_color); + else + { + t.setColor(mrpt::img::colormap( + renderOptions.planes_colormap, + recolorK * + (center[renderOptions.recolorizeByCoordinateIndex] - + recolorMin))); + } + t.vertices[0].xyzrgba.pt = center + vx + vy; t.vertices[1].xyzrgba.pt = center - vx - vy; t.vertices[2].xyzrgba.pt = center + vx - vy; @@ -282,6 +350,7 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const outObj.insert(obj); } + // normals: if (renderOptions.normals_visible) { auto obj = mrpt::opengl::CSetOfLines::Create(); @@ -302,7 +371,7 @@ void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const // eigenVector[0] is the plane normal. // [1] and [2] are parallel to the plane surface: - const float s = voxel_size_ * 0.5f; + const float s = voxel_size_ * 0.2f; const auto vz = ndt->eigVectors.at(0).cast() * s; const auto p1 = center; @@ -777,11 +846,14 @@ void NDT::TLikelihoodOptions::readFromStream(mrpt::serialization::CArchive& in) void NDT::TRenderOptions::writeToStream( mrpt::serialization::CArchive& out) const { - const int8_t version = 0; + const int8_t version = 1; out << version; out << points_visible << point_size << points_color; out << planes_visible << planes_color; out << normals_visible << normals_color; + out << static_cast(points_colormap) + << static_cast(planes_colormap) + << recolorizeByCoordinateIndex; // v1 } void NDT::TRenderOptions::readFromStream(mrpt::serialization::CArchive& in) @@ -791,10 +863,17 @@ void NDT::TRenderOptions::readFromStream(mrpt::serialization::CArchive& in) switch (version) { case 0: + case 1: { in >> points_visible >> point_size >> points_color; in >> planes_visible >> planes_color; in >> normals_visible >> normals_color; + if (version >= 1) + { + in.ReadAsAndCastTo(this->points_colormap); + in.ReadAsAndCastTo(this->planes_colormap); + in >> recolorizeByCoordinateIndex; + } } break; default: @@ -842,6 +921,10 @@ void NDT::TRenderOptions::dumpToTextStream(std::ostream& out) const LOADABLEOPTS_DUMP_VAR(normals_color.R, float); LOADABLEOPTS_DUMP_VAR(normals_color.G, float); LOADABLEOPTS_DUMP_VAR(normals_color.B, float); + + LOADABLEOPTS_DUMP_VAR(points_colormap, int); + LOADABLEOPTS_DUMP_VAR(planes_colormap, int); + LOADABLEOPTS_DUMP_VAR(recolorizeByCoordinateIndex, int); } void NDT::TInsertionOptions::loadFromConfigFile( @@ -879,6 +962,10 @@ void NDT::TRenderOptions::loadFromConfigFile( MRPT_LOAD_CONFIG_VAR(normals_color.R, float, c, s); MRPT_LOAD_CONFIG_VAR(normals_color.G, float, c, s); MRPT_LOAD_CONFIG_VAR(normals_color.B, float, c, s); + + points_colormap = c.read_enum(s, "points_colormap", this->points_colormap); + planes_colormap = c.read_enum(s, "planes_colormap", this->planes_colormap); + MRPT_LOAD_CONFIG_VAR(recolorizeByCoordinateIndex, int, c, s); } void NDT::internal_insertPointCloud3D( diff --git a/mola_msgs/CHANGELOG.rst b/mola_msgs/CHANGELOG.rst index 2d8879618e..0ecfe643ea 100644 --- a/mola_msgs/CHANGELOG.rst +++ b/mola_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_msgs/package.xml b/mola_msgs/package.xml index de9bb62208..742e77fef3 100644 --- a/mola_msgs/package.xml +++ b/mola_msgs/package.xml @@ -2,7 +2,7 @@ mola_msgs - 1.2.1 + 1.3.0 ROS message, services, and actions used in other MOLA packages. Jose Luis Blanco-Claraco diff --git a/mola_navstate_fg/CHANGELOG.rst b/mola_navstate_fg/CHANGELOG.rst index d3ccd98110..c390ad0599 100644 --- a/mola_navstate_fg/CHANGELOG.rst +++ b/mola_navstate_fg/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mola_navstate_fg ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* Start integrating GNSS observation. Added a new CLI program mola-navstate-cli for testing state fusion +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_navstate_fg/CMakeLists.txt b/mola_navstate_fg/CMakeLists.txt index 95fd16b1c2..80878d28c7 100644 --- a/mola_navstate_fg/CMakeLists.txt +++ b/mola_navstate_fg/CMakeLists.txt @@ -45,6 +45,15 @@ mola_add_library( ) target_include_directories(${PROJECT_NAME} PRIVATE ".") +# ----------------------- +# define cli apps: +mola_add_executable( + TARGET mola-navstate-cli + SOURCES apps/mola-navstate-cli.cpp + LINK_LIBRARIES + ${PROJECT_NAME} +) + # ----------------------- # define tests: enable_testing() diff --git a/mola_navstate_fg/apps/mola-navstate-cli.cpp b/mola_navstate_fg/apps/mola-navstate-cli.cpp new file mode 100644 index 0000000000..3e811d29d1 --- /dev/null +++ b/mola_navstate_fg/apps/mola-navstate-cli.cpp @@ -0,0 +1,93 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ + +#include +#include +#include +#include +#include +#include + +#include + +namespace +{ +void run_navstate(const std::string& paramsFile, const std::string& rawlogFile) +{ + using mrpt::obs::CObservationGPS; + using mrpt::obs::CObservationIMU; + using mrpt::obs::CObservationRobotPose; + + mola::NavStateFG nav; + + std::cout << "Initalizing from: " << paramsFile << std::endl; + + const auto cfg = mrpt::containers::yaml::FromFile(paramsFile); + nav.initialize(cfg); + + std::cout << "Reading dataset from: " << rawlogFile << std::endl; + + mrpt::obs::CRawlog dataset; + dataset.loadFromRawLogFile(rawlogFile); + + const std::string frame_id = "map"; + + std::cout << "Read entries: " << dataset.size() << std::endl; + + for (size_t i = 0; i < dataset.size(); i++) + { + const auto o = dataset.getAsObservation(i); + if (!o) continue; + + if (auto oGPS = std::dynamic_pointer_cast(o); oGPS) + { + nav.fuse_gnss(*oGPS); + } + else if (auto oPose = + std::dynamic_pointer_cast(o); + oPose) + { + nav.fuse_pose(oPose->timestamp, oPose->pose, frame_id); + } + else if (auto oImu = std::dynamic_pointer_cast(o); + oImu) + { + nav.fuse_imu(*oImu); + } + else + { + std::cout << "[Warning] Ignoring observation #" << i << ": '" + << o->sensorLabel + << "' of type: " << o->GetRuntimeClass()->className + << "\n"; + } + + } // for each entry +} + +} // namespace + +int main(int argc, char** argv) +{ + try + { + if (argc != 3) + { + std::cerr << "Usage: " << argv[0] << " params.yaml dataset.rawlog" + << std::endl; + return 1; + } + + run_navstate(argv[1], argv[2]); + + return 0; + } + catch (const std::exception& e) + { + std::cerr << "Exception: " << e.what() << std::endl; + return 1; + } +} diff --git a/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h b/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h index 4c23812ae0..14a958c1a1 100644 --- a/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h +++ b/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h @@ -128,6 +128,9 @@ class NavStateFG : public mola::NavStateFilter /** Integrates new IMU observations into the estimator */ void fuse_imu(const mrpt::obs::CObservationIMU& imu) override; + /** Integrates new GNSS observations into the estimator */ + void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override; + /** Integrates new twist estimation (in the odom frame) */ void fuse_twist( const mrpt::Clock::time_point& timestamp, diff --git a/mola_navstate_fg/package.xml b/mola_navstate_fg/package.xml index 10e9aa9c45..e98fd3ab1d 100644 --- a/mola_navstate_fg/package.xml +++ b/mola_navstate_fg/package.xml @@ -5,7 +5,7 @@ --> mola_navstate_fg - 1.2.1 + 1.3.0 SE(3) pose and twist path data fusion estimator Jose-Luis Blanco-Claraco diff --git a/mola_navstate_fg/src/NavStateFG.cpp b/mola_navstate_fg/src/NavStateFG.cpp index 3bce77076f..4c7633f16c 100644 --- a/mola_navstate_fg/src/NavStateFG.cpp +++ b/mola_navstate_fg/src/NavStateFG.cpp @@ -154,6 +154,14 @@ void NavStateFG::fuse_imu(const mrpt::obs::CObservationIMU& imu) delete_too_old_entries(); } +void NavStateFG::fuse_gnss(const mrpt::obs::CObservationGPS& gps) +{ + THROW_EXCEPTION("TODO"); + (void)gps; + + delete_too_old_entries(); +} + void NavStateFG::fuse_pose( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose, const std::string& frame_id) diff --git a/mola_navstate_fg/tests/mola-navstate-fg-params.yaml b/mola_navstate_fg/tests/mola-navstate-fg-params.yaml new file mode 100644 index 0000000000..3b849a8ec7 --- /dev/null +++ b/mola_navstate_fg/tests/mola-navstate-fg-params.yaml @@ -0,0 +1,10 @@ +# Config for NavStateFGParams +sliding_window_length: 5.0 # [s] +max_time_to_use_velocity_model: 2.0 # [s] +time_between_frames_to_warning: 2.0 # [s] +sigma_random_walk_acceleration_linear: 1.0 # [m/s²] +sigma_random_walk_acceleration_angular: 1.0 # [rad/s²] +sigma_integrator_position: 0.10 # [m] +sigma_integrator_orientation: 0.10 # [rad] +robust_param: 0 +max_rmse: 2 diff --git a/mola_navstate_fuse/CHANGELOG.rst b/mola_navstate_fuse/CHANGELOG.rst index e1e1ed3061..151eaf8e5e 100644 --- a/mola_navstate_fuse/CHANGELOG.rst +++ b/mola_navstate_fuse/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mola_navstate_fuse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* Start integrating GNSS observation. Added a new CLI program mola-navstate-cli for testing state fusion +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h index 2291feb9d5..10f321d7f2 100644 --- a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h +++ b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h @@ -98,6 +98,9 @@ class NavStateFuse : public mola::NavStateFilter /** Integrates new IMU observations into the estimator */ void fuse_imu(const mrpt::obs::CObservationIMU& imu) override; + /** Integrates new GNSS observations into the estimator */ + void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override; + /** Integrates new twist estimation (in the odom frame) */ void fuse_twist( const mrpt::Clock::time_point& timestamp, diff --git a/mola_navstate_fuse/package.xml b/mola_navstate_fuse/package.xml index 169c0fad9a..303e07afa1 100644 --- a/mola_navstate_fuse/package.xml +++ b/mola_navstate_fuse/package.xml @@ -5,7 +5,7 @@ --> mola_navstate_fuse - 1.2.1 + 1.3.0 SE(3) pose and twist path data fusion estimator Jose-Luis Blanco-Claraco diff --git a/mola_navstate_fuse/src/NavStateFuse.cpp b/mola_navstate_fuse/src/NavStateFuse.cpp index 5359cbb54a..88d19e3d99 100644 --- a/mola_navstate_fuse/src/NavStateFuse.cpp +++ b/mola_navstate_fuse/src/NavStateFuse.cpp @@ -70,6 +70,12 @@ void NavStateFuse::fuse_imu(const mrpt::obs::CObservationIMU& imu) (void)imu; } +void NavStateFuse::fuse_gnss(const mrpt::obs::CObservationGPS& gps) +{ + // TODO(jlbc) + (void)gps; +} + void NavStateFuse::fuse_pose( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose, diff --git a/mola_pose_list/CHANGELOG.rst b/mola_pose_list/CHANGELOG.rst index f56d8a68a3..09ac760e63 100644 --- a/mola_pose_list/CHANGELOG.rst +++ b/mola_pose_list/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_pose_list ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_pose_list/package.xml b/mola_pose_list/package.xml index e70b1ad064..89909dc21c 100644 --- a/mola_pose_list/package.xml +++ b/mola_pose_list/package.xml @@ -5,7 +5,7 @@ --> mola_pose_list - 1.2.1 + 1.3.0 C++ library for searchable pose lists Jose-Luis Blanco-Claraco diff --git a/mola_relocalization/CHANGELOG.rst b/mola_relocalization/CHANGELOG.rst index 9e9a703281..47fa6a12c2 100644 --- a/mola_relocalization/CHANGELOG.rst +++ b/mola_relocalization/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package mola_relocalization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_relocalization/package.xml b/mola_relocalization/package.xml index 44300bb51b..aae3c65928 100644 --- a/mola_relocalization/package.xml +++ b/mola_relocalization/package.xml @@ -5,7 +5,7 @@ --> mola_relocalization - 1.2.1 + 1.3.0 C++ library with algorithms for relocalization, global localization, or pose estimation given a large initial uncertainty Jose-Luis Blanco-Claraco diff --git a/mola_traj_tools/CHANGELOG.rst b/mola_traj_tools/CHANGELOG.rst index 2785f94358..678652102c 100644 --- a/mola_traj_tools/CHANGELOG.rst +++ b/mola_traj_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_traj_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_traj_tools/package.xml b/mola_traj_tools/package.xml index 44bceb2518..f98681936d 100644 --- a/mola_traj_tools/package.xml +++ b/mola_traj_tools/package.xml @@ -5,7 +5,7 @@ --> mola_traj_tools - 1.2.1 + 1.3.0 CLI tools to manipulate trajectory files as a complement to the evo package Jose-Luis Blanco-Claraco diff --git a/mola_viz/CHANGELOG.rst b/mola_viz/CHANGELOG.rst index 795d9c7eb1..71a4c69a1c 100644 --- a/mola_viz/CHANGELOG.rst +++ b/mola_viz/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mola_viz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ +* mola_viz: Show IMU data in the GUI too +* Contributors: Jose Luis Blanco-Claraco + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_viz/package.xml b/mola_viz/package.xml index ac43b55f14..3387913c78 100644 --- a/mola_viz/package.xml +++ b/mola_viz/package.xml @@ -5,7 +5,7 @@ --> mola_viz - 1.2.1 + 1.3.0 GUI for MOLA Jose-Luis Blanco-Claraco diff --git a/mola_viz/src/MolaViz.cpp b/mola_viz/src/MolaViz.cpp index 50f13f40f0..8bae2040be 100644 --- a/mola_viz/src/MolaViz.cpp +++ b/mola_viz/src/MolaViz.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -482,6 +483,71 @@ void gui_handler_gps( } } +// CObservationIMU +void gui_handler_imu( + const mrpt::rtti::CObject::Ptr& o, nanogui::Window* w, + MolaViz::window_name_t parentWin, MolaViz* instance) +{ + auto obj = std::dynamic_pointer_cast(o); + if (!obj) return; + + mrpt::gui::MRPT2NanoguiGLCanvas* glControl; + // mrpt::opengl::CSetOfObjects::Ptr glCornerRef, glCornerSensor; + std::optional> lck; + + if (w->children().size() == 1) + { + // Create on first use: + w->setLayout(new nanogui::GridLayout( + nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 2, + 2)); + + glControl = w->add(); + + lck.emplace(&glControl->scene_mtx); + + glControl->scene = mrpt::opengl::COpenGLScene::Create(); + + // glCornerRef = mrpt::opengl::stock_objects::CornerXYZ(1.0f); + // glControl->scene->insert(glCornerRef); + + const int winW = 400, winH = 125; + glControl->setSize({winW, winH}); + glControl->setFixedSize({winW, winH}); + + instance->markWindowForReLayout(parentWin); + } + else + { + // Reuse from past iterations: + glControl = + dynamic_cast(w->children().at(1)); + lck.emplace(&glControl->scene_mtx); + + // glCornerRef + // =glControl->scene->getByClass(0); + } + ASSERT_(glControl != nullptr); + + std::vector txts; + + if (obj->has(mrpt::obs::IMU_WX)) + txts.push_back(mrpt::format( + "omega=(%7.04f,%7.04f,%7.04f)", obj->get(mrpt::obs::IMU_WX), + obj->get(mrpt::obs::IMU_WY), obj->get(mrpt::obs::IMU_WZ))); + else + txts.push_back("omega=None"); + + if (obj->has(mrpt::obs::IMU_X_ACC)) + txts.push_back(mrpt::format( + "acc=(%7.04f,%7.04f,%7.04f)", obj->get(mrpt::obs::IMU_X_ACC), + obj->get(mrpt::obs::IMU_Y_ACC), obj->get(mrpt::obs::IMU_Z_ACC))); + else + txts.push_back("acc=None"); + + gui_handler_show_common_sensor_info(*obj, w, txts); +} + } // namespace MRPT_INITIALIZER(do_register_MolaViz) @@ -493,6 +559,7 @@ MRPT_INITIALIZER(do_register_MolaViz) // clang-format off MolaViz::register_gui_handler("mrpt::obs::CObservationImage", &gui_handler_images); MolaViz::register_gui_handler("mrpt::obs::CObservationGPS", &gui_handler_gps); + MolaViz::register_gui_handler("mrpt::obs::CObservationIMU", &gui_handler_imu); MolaViz::register_gui_handler("mrpt::obs::CObservationPointCloud", &gui_handler_point_cloud); MolaViz::register_gui_handler("mrpt::obs::CObservation3DRangeScan", &gui_handler_point_cloud); MolaViz::register_gui_handler("mrpt::obs::CObservation3DRangeScan", &gui_handler_images); diff --git a/mola_yaml/CHANGELOG.rst b/mola_yaml/CHANGELOG.rst index 1caf296d2f..ed738bb593 100644 --- a/mola_yaml/CHANGELOG.rst +++ b/mola_yaml/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mola_yaml ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.3.0 (2024-12-11) +------------------ + 1.2.1 (2024-09-29) ------------------ diff --git a/mola_yaml/package.xml b/mola_yaml/package.xml index 630033341a..d8a81769aa 100644 --- a/mola_yaml/package.xml +++ b/mola_yaml/package.xml @@ -5,7 +5,7 @@ --> mola_yaml - 1.2.1 + 1.3.0 YAML helper library common to MOLA modules Jose-Luis Blanco-Claraco diff --git a/scripts/clang_git_format/clang_git_format/config.py b/scripts/clang_git_format/clang_git_format/config.py index c834781f71..c969020654 100644 --- a/scripts/clang_git_format/clang_git_format/config.py +++ b/scripts/clang_git_format/clang_git_format/config.py @@ -1,2 +1,2 @@ -PROGNAME = "clang-format-11" +PROGNAME = "clang-format-14" diff --git a/scripts/generate_statuses_table.py b/scripts/generate_statuses_table.py index 0d23d32a5b..63cf24d0e6 100755 --- a/scripts/generate_statuses_table.py +++ b/scripts/generate_statuses_table.py @@ -38,10 +38,6 @@ def process_directories(path, items_to_remove, pattern_text): "
" \ "[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__{name}__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__{name}__ubuntu_jammy_arm64__binary/) " \ " | " \ - "[![Build Status](https://build.ros2.org/job/Ibin_uJ64__{name}__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uJ64__{name}__ubuntu_jammy_amd64__binary/) " \ - "
" \ - "[![Build Status](https://build.ros2.org/job/Ibin_ujv8_uJv8__{name}__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Ibin_ujv8_uJv8__{name}__ubuntu_jammy_arm64__binary/) " \ - " | " \ "[![Build Status](https://build.ros2.org/job/Jbin_uN64__{name}__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__{name}__ubuntu_noble_amd64__binary/) " \ "
" \ "[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__{name}__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__{name}__ubuntu_noble_arm64__binary/) " \