From 2d8d149dc1fe667ea89dde6f329aa5537feb615f Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 18 Aug 2024 22:43:51 +0200 Subject: [PATCH] Imported upstream version '1.1.0' of 'upstream' --- .clang-format | 2 +- .github/workflows/build-ros.yml | 11 +- README.md | 14 +- docs/source/building-maps.rst | 2 + docs/source/index.rst | 78 +++- docs/source/refs.bib | 1 + docs/source/solutions.rst | 36 +- kitti_metrics_eval/CHANGELOG.rst | 7 + kitti_metrics_eval/package.xml | 5 +- mola/CHANGELOG.rst | 8 + mola/package.xml | 9 +- mola_bridge_ros2/.clang-format | 2 +- mola_bridge_ros2/CHANGELOG.rst | 13 + mola_bridge_ros2/CMakeLists.txt | 5 + .../include/mola_bridge_ros2/BridgeROS2.h | 75 ++-- mola_bridge_ros2/package.xml | 14 +- mola_bridge_ros2/src/BridgeROS2.cpp | 394 +++++++++++------- mola_demos/CHANGELOG.rst | 7 + mola_demos/package.xml | 7 +- mola_imu_preintegration/CHANGELOG.rst | 8 + mola_imu_preintegration/package.xml | 5 +- .../tests/test-imu-rotation-integrator.cpp | 3 +- mola_input_euroc_dataset/CHANGELOG.rst | 8 + mola_input_euroc_dataset/package.xml | 5 +- mola_input_euroc_dataset/src/EurocDataset.cpp | 16 +- mola_input_kitti360_dataset/CHANGELOG.rst | 7 + mola_input_kitti360_dataset/package.xml | 5 +- mola_input_kitti_dataset/CHANGELOG.rst | 7 + mola_input_kitti_dataset/package.xml | 5 +- mola_input_mulran_dataset/CHANGELOG.rst | 8 + mola_input_mulran_dataset/package.xml | 5 +- .../src/MulranDataset.cpp | 2 +- mola_input_paris_luco_dataset/CHANGELOG.rst | 8 + mola_input_paris_luco_dataset/package.xml | 5 +- .../src/ParisLucoDataset.cpp | 6 +- mola_input_rawlog/CHANGELOG.rst | 8 + mola_input_rawlog/package.xml | 5 +- mola_input_rawlog/src/RawlogDataset.cpp | 86 ++-- mola_input_rosbag2/CHANGELOG.rst | 7 + mola_input_rosbag2/package.xml | 5 +- mola_kernel/CHANGELOG.rst | 14 + mola_kernel/CMakeLists.txt | 92 +++- .../mola_kernel/interfaces/MapServer.h | 65 +++ mola_kernel/include/mola_kernel/version.h | 19 + mola_kernel/package.xml | 5 +- mola_kernel/src/Entity.cpp | 27 +- mola_kernel/src/Factor.cpp | 6 +- .../src/factors/FactorConstVelKinematics.cpp | 2 +- .../factors/FactorStereoProjectionPose.cpp | 2 +- mola_kernel/src/interfaces/FilterBase.cpp | 3 +- mola_kernel/src/interfaces/MapServer.cpp | 19 + .../src/interfaces/RawDataSourceBase.cpp | 6 +- mola_kernel/src/pretty_print_exception.cpp | 6 +- mola_launcher/CHANGELOG.rst | 8 + mola_launcher/package.xml | 5 +- mola_launcher/src/MolaDLL_Loader.cpp | 2 + mola_launcher/src/MolaDLL_Loader.h | 1 + mola_metric_maps/CHANGELOG.rst | 8 + mola_metric_maps/apps/mola-myapp.cpp | 13 +- mola_metric_maps/package.xml | 5 +- .../src/SparseTreesPointCloud.cpp | 49 ++- .../src/SparseVoxelPointCloud.cpp | 80 ++-- .../test-mola_metric_maps_hashedvoxels.cpp | 9 +- .../tests/test-sparsevoxelpointcloud.cpp | 14 +- mola_msgs/CHANGELOG.rst | 10 + mola_msgs/CMakeLists.txt | 2 + mola_msgs/package.xml | 4 +- mola_msgs/srv/MapLoad.srv | 12 + mola_msgs/srv/MapSave.srv | 12 + mola_navstate_fg/CHANGELOG.rst | 8 + mola_navstate_fg/package.xml | 7 +- .../tests/test-navstate-basic.cpp | 4 +- mola_navstate_fuse/CHANGELOG.rst | 7 + mola_navstate_fuse/package.xml | 5 +- mola_pose_list/CHANGELOG.rst | 7 + mola_pose_list/package.xml | 5 +- mola_relocalization/CHANGELOG.rst | 7 + mola_relocalization/package.xml | 5 +- mola_traj_tools/CHANGELOG.rst | 8 + mola_traj_tools/package.xml | 5 +- mola_traj_tools/src/traj_ypr2tum.cpp | 3 +- mola_viz/CHANGELOG.rst | 8 + mola_viz/package.xml | 5 +- mola_viz/src/mola_icon_64x64.h | 18 +- mola_yaml/CHANGELOG.rst | 8 + mola_yaml/package.xml | 5 +- mola_yaml/src/yaml_helpers.cpp | 5 +- scripts/clang-formatter.sh | 3 + 88 files changed, 1067 insertions(+), 460 deletions(-) create mode 100644 mola_kernel/include/mola_kernel/interfaces/MapServer.h create mode 100644 mola_kernel/include/mola_kernel/version.h create mode 100644 mola_kernel/src/interfaces/MapServer.cpp create mode 100644 mola_msgs/srv/MapLoad.srv create mode 100644 mola_msgs/srv/MapSave.srv create mode 100755 scripts/clang-formatter.sh diff --git a/.clang-format b/.clang-format index d1df9d2871..1d8267c6e1 100644 --- a/.clang-format +++ b/.clang-format @@ -36,7 +36,7 @@ AllowShortBlocksOnASingleLine: true BreakBeforeBraces: Allman #BreakBeforeTernaryOperators: true #BreakConstructorInitializersBeforeComma: false -ColumnLimit: 80 +ColumnLimit: 100 #CommentPragmas: '' #ConstructorInitializerAllOnOneLineOrOnePerLine: true #ConstructorInitializerIndentWidth: 4 diff --git a/.github/workflows/build-ros.yml b/.github/workflows/build-ros.yml index 810b492a23..1232921f78 100644 --- a/.github/workflows/build-ros.yml +++ b/.github/workflows/build-ros.yml @@ -1,7 +1,7 @@ # Based on GTSAM file (by @ProfFan) name: CI Build colcon -on: [push, pull_request] +on: [push] jobs: build_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. @@ -42,9 +42,9 @@ jobs: ros_version: 2 # Rolling Ridley (No End-Of-Life) -# - docker_image: ubuntu:noble -# ros_distribution: rolling -# ros_version: 2 + - docker_image: ubuntu:noble + ros_distribution: rolling + ros_version: 2 container: image: ${{ matrix.docker_image }} @@ -55,6 +55,9 @@ jobs: apt-get -y update apt-get -y install git git clone https://github.com/$GITHUB_REPOSITORY.git --recursive "$GITHUB_WORKSPACE" + git config --global --add safe.directory "$GITHUB_WORKSPACE" + git checkout $GITHUB_SHA + git submodule update --init --recursive - name: setup ROS environment uses: ros-tooling/setup-ros@v0.7 diff --git a/README.md b/README.md index f8a1460428..ee94039228 100644 --- a/README.md +++ b/README.md @@ -50,9 +50,19 @@ Refer to the [official documentation](https://docs.mola-slam.org/latest/) for bu ## Citations -The latest publications on MOLA are: +The latest publications on MOLA are ([PDF](https://arxiv.org/abs/2407.20465)). - mola_lidar_odometry: (TBD 2024-Q2!!) +```bibtex +@misc{blanco2024mola, + title={A flexible framework for accurate LiDAR odometry, map manipulation, and localization}, + author={José Luis Blanco-Claraco}, + year={2024}, + eprint={2407.20465}, + archivePrefix={arXiv}, + primaryClass={cs.RO}, + url={https://arxiv.org/abs/2407.20465}, +} +``` MOLA was initially presented in 2019 in ([PDF](http://www.roboticsproceedings.org/rss15/p43.pdf)): diff --git a/docs/source/building-maps.rst b/docs/source/building-maps.rst index 854a194a92..c3eeacec91 100644 --- a/docs/source/building-maps.rst +++ b/docs/source/building-maps.rst @@ -118,6 +118,8 @@ or from the :ref:`UI controls ` in the ``mola_lidar_od | +.. _building-maps_sect_inspect_sm: + 3. Inspect the resulting simple-map ---------------------------------------- To verify that the generated simple-map is correct, you can use :ref:`sm-cli `. diff --git a/docs/source/index.rst b/docs/source/index.rst index b2e03ded09..8216acd76a 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -53,7 +53,7 @@ MOLA :octicon:`mark-github` `MOLA`_ is a Modular system for Localization and Mapping. Get started: - - Read the tutorial: :ref:`building-maps`. + - Read: :ref:`building-maps`. - Read :octicon:`rocket` :ref:`mola_lidar_odometry` documentation. - See :ref:`use-cases` for examples of use. - See :ref:`installing` and :ref:`how to cite it `. @@ -67,6 +67,55 @@ Get started: | +.. humble badges ------ + +.. |badgeHdev| image:: https://build.ros2.org/job/Hdev__mola__ubuntu_jammy_amd64/badge/icon + :scale: 100% + :align: middle + :target: https://build.ros2.org/job/Hdev__mola__ubuntu_jammy_amd64/ + +.. |badgeHrel| image:: https://img.shields.io/ros/v/humble/mola + :scale: 100% + :align: middle + :target: https://index.ros.org/search/?term=mola + +.. iron badges ------ + +.. |badgeIdev| image:: https://build.ros2.org/job/Idev__mola__ubuntu_jammy_amd64/badge/icon + :scale: 100% + :align: middle + :target: https://build.ros2.org/job/Idev__mola__ubuntu_jammy_amd64/ + +.. |badgeIrel| image:: https://img.shields.io/ros/v/iron/mola + :scale: 100% + :align: middle + :target: https://index.ros.org/search/?term=mola + +.. jazzy badges ------ + +.. |badgeJdev| image:: https://build.ros2.org/job/Jdev__mola__ubuntu_noble_amd64/badge/icon + :scale: 100% + :align: middle + :target: https://build.ros2.org/job/Jdev__mola__ubuntu_noble_amd64/ + +.. |badgeJrel| image:: https://img.shields.io/ros/v/jazzy/mola + :scale: 100% + :align: middle + :target: https://index.ros.org/search/?term=mola + +.. rolling badges ------ + +.. |badgeRdev| image:: https://build.ros2.org/job/Rdev__mola__ubuntu_noble_amd64/badge/icon + :scale: 100% + :align: middle + :target: https://build.ros2.org/job/Rdev__mola__ubuntu_noble_amd64/ + +.. |badgeRrel| image:: https://img.shields.io/ros/v/rolling/mola + :scale: 100% + :align: middle + :target: https://index.ros.org/search/?term=mola + + .. _installing: Installing @@ -89,7 +138,9 @@ How to install all MOLA modules: sudo apt install ros-$ROS_DISTRO-mola # Install the MOLA LIDAR odometry package: - sudo apt install ros-$ROS_DISTRO-mola-lidar-odometry + # sudo apt install ros-$ROS_DISTRO-mola-lidar-odometry + # As of Jul 2024, this package is not available from apt yet! + # Please see instructions below to clone and build it from sources # Install example small datasets to run demos/unit tests: sudo apt install ros-$ROS_DISTRO-mola-test-datasets @@ -98,15 +149,23 @@ How to install all MOLA modules: .. code-block:: bash - cd ~/ros2_mola_ws/ - . install/setup.bash - # For example, let's launch the mm map viewer. # If a GUI app is opened, it means installation was successful. mm-viewer - Check the `build status table `_ to find out - what MOLA version is available for your ROS distribution. + These are the versions available from ROS build farms: + + +-------------------------+-----------------------------+------------------------------------+ + | ROS distribution | Development build status | Last release (available via apt) | + +=========================+=============================+====================================+ + | ROS 2 Humble (u22.04) | |badgeHdev| | |badgeHrel| | + +-------------------------+-----------------------------+------------------------------------+ + | ROS 2 Iron (u22.04) | |badgeIdev| | |badgeIrel| | + +-------------------------+-----------------------------+------------------------------------+ + | ROS 2 Jazzy (u24.04) | |badgeJdev| | |badgeJrel| | + +-------------------------+-----------------------------+------------------------------------+ + | ROS 2 Rolling (u24.04) | |badgeRdev| | |badgeRrel| | + +-------------------------+-----------------------------+------------------------------------+ .. dropdown:: Build from sources @@ -129,9 +188,6 @@ How to install all MOLA modules: mkdir -p ~/ros2_mola_ws/src/ cd ~/ros2_mola_ws/src/ - # Optional: Get latest version of mrpt2. - # git clone https://github.com/MRPT/mrpt.git mrpt2 --recursive - # Main MOLA modules: git clone https://github.com/MOLAorg/mola_common.git git clone https://github.com/MOLAorg/mp2p_icp.git --recursive @@ -186,7 +242,7 @@ The ``mola_lidar_odometry`` system was presented in :cite:`blanco2024mola_lo`: `A flexible framework for accurate LiDAR odometry, map manipulation, and localization`_, in ArXiV, 2024. -.. _A flexible framework for accurate LiDAR odometry, map manipulation, and localization: https://TBD +.. _A flexible framework for accurate LiDAR odometry, map manipulation, and localization: https://arxiv.org/abs/2407.20465 The basics of the MOLA framework were introduced in :cite:`blanco2019modular`. diff --git a/docs/source/refs.bib b/docs/source/refs.bib index d651ed4868..5bc8ffa9ee 100644 --- a/docs/source/refs.bib +++ b/docs/source/refs.bib @@ -55,6 +55,7 @@ @article{blanco2024mola_lo title = {{A flexible framework for accurate LiDAR odometry, map manipulation, and localization}}, journal = {ArXiV}, year = {2024}, + url = {https://arxiv.org/abs/2407.20465} } @article{blanco2018olae, diff --git a/docs/source/solutions.rst b/docs/source/solutions.rst index 99892180d2..bcbe045ed4 100644 --- a/docs/source/solutions.rst +++ b/docs/source/solutions.rst @@ -1,14 +1,14 @@ .. _solutions: ========================= -Solutions and pricing +Solutions and licensing ========================= Solutions =============== -1. Flexible LIDAR odometry and Localization ------------------------------------------- +1. Flexible 3D LIDAR odometry and Localization +------------------------------------------------ :ref:`LiDAR odometry ` is one of the most advanced and flexible LIDAR odometry modules out there. Check out the tutorial: :ref:`building-maps`. @@ -17,17 +17,39 @@ Check out the tutorial: :ref:`building-maps`. | +2. Full 3D SLAM solution (GNSS, submapping, loop closures) +------------------------------------------------------------ +Build **geo-referenced** consistent global maps, even mixing indoor and outdoor scenarios. +This functionality is provided by: -2. Full SLAM solution (GNSS, submapping, loop closures) --------------------------------------------------------- +- ``mola_sm_loop_closure``: TO-DO: add docs. + At present, this package is not open-sourced, see: :ref:`mola_licenses`. + + - **Geo-referencing** metric maps with consumer-grade GNSS sensors. + - Off-line **loop closure** for consistent global maps. + +- ``mola_3d_lidar_slam``: (Coming soon!) -Build **geo-referenced** consistent global maps, even mixing indoor and outdoor scenarios. .. image:: https://mrpt.github.io/imgs/kaist01_georef_sample.png + +| + +3. Full 2D SLAM solution +---------------------------- + +Build **geo-referenced** consistent global 2D maps from 2D LiDARs. +This functionality is provided by: + +- ``mola_2d_lidar_slam``: (Coming soon!) + + | +.. _mola_licenses: + License and pricing ===================== The complete framework comprises these software repositories: @@ -91,7 +113,7 @@ The complete framework comprises these software repositories: Contact =========== -To request details on **licensing a closed-source version for commercial usages** and/or **consulting services**, please use the contact form below: +To request details on **licensing a closed-source version for commercial usages** and/or **consulting services**, please use `this contact form `_: .. raw:: html diff --git a/kitti_metrics_eval/CHANGELOG.rst b/kitti_metrics_eval/CHANGELOG.rst index e20c6e7a67..211a45aac6 100644 --- a/kitti_metrics_eval/CHANGELOG.rst +++ b/kitti_metrics_eval/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package kitti_metrics_eval ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/kitti_metrics_eval/package.xml b/kitti_metrics_eval/package.xml index 5750acef6d..6e1572ecf8 100644 --- a/kitti_metrics_eval/package.xml +++ b/kitti_metrics_eval/package.xml @@ -5,15 +5,14 @@ --> kitti_metrics_eval - 1.0.8 + 1.1.0 CLI tool to evaluate the KITTI odometry bechmark metrics to trajectory files - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + BSD https://github.com/MOLAorg/mola/tree/develop/kitti_metrics_eval - BSD mola_common mrpt2 diff --git a/mola/CHANGELOG.rst b/mola/CHANGELOG.rst index ff58863c8d..23bc9fa026 100644 --- a/mola/CHANGELOG.rst +++ b/mola/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* change ament linters to apply in test builds +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ diff --git a/mola/package.xml b/mola/package.xml index a4d56aa611..d1b1963edf 100644 --- a/mola/package.xml +++ b/mola/package.xml @@ -2,10 +2,10 @@ mola - 1.0.8 + 1.1.0 Metapackage with all core open-sourced MOLA packages. + Jose-Luis Blanco-Claraco - Jose-Luis Blanco-Claraco BSD ament_cmake @@ -32,8 +32,9 @@ mola_viz mola_yaml - ament_lint_common - ament_lint_auto + ament_lint_auto + ament_cmake_xmllint + ament_lint_cmake ament_cmake diff --git a/mola_bridge_ros2/.clang-format b/mola_bridge_ros2/.clang-format index d1df9d2871..1d8267c6e1 100644 --- a/mola_bridge_ros2/.clang-format +++ b/mola_bridge_ros2/.clang-format @@ -36,7 +36,7 @@ AllowShortBlocksOnASingleLine: true BreakBeforeBraces: Allman #BreakBeforeTernaryOperators: true #BreakConstructorInitializersBeforeComma: false -ColumnLimit: 80 +ColumnLimit: 100 #CommentPragmas: '' #ConstructorInitializerAllOnOneLineOrOnePerLine: true #ConstructorInitializerIndentWidth: 4 diff --git a/mola_bridge_ros2/CHANGELOG.rst b/mola_bridge_ros2/CHANGELOG.rst index a57bc06f70..bff6b3eb89 100644 --- a/mola_bridge_ros2/CHANGELOG.rst +++ b/mola_bridge_ros2/CHANGELOG.rst @@ -3,6 +3,19 @@ Changelog for package mola_bridge_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#65 `_ from MOLAorg/add-more-srvs + Add more Services +* Offer ROS2 services for the new MOLA MapServer interface +* clang-format: switch to 100 columns +* ros2bridge: offer ROS2 services for relocalization +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* change ament linters to apply in test builds +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_bridge_ros2/CMakeLists.txt b/mola_bridge_ros2/CMakeLists.txt index ae54e9f933..7e32f42317 100644 --- a/mola_bridge_ros2/CMakeLists.txt +++ b/mola_bridge_ros2/CMakeLists.txt @@ -28,6 +28,7 @@ endif() # Find MOLA pkgs: find_mola_package(mola_kernel) + # Find ROS 2 pkgs: mola_find_package_or_return(ament_cmake REQUIRED) mola_find_package_or_return(geometry_msgs REQUIRED) @@ -38,6 +39,9 @@ mola_find_package_or_return(sensor_msgs) mola_find_package_or_return(tf2 REQUIRED) mola_find_package_or_return(tf2_geometry_msgs REQUIRED) +# MOLA ROS2 services: +mola_find_package_or_return(mola_msgs REQUIRED) + # ----------------------- # define lib: file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h) @@ -63,6 +67,7 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC sensor_msgs tf2 tf2_geometry_msgs + mola_msgs ) ament_export_dependencies() diff --git a/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h b/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h index cac8752dd3..f3ad5c0a45 100644 --- a/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h +++ b/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h @@ -14,9 +14,11 @@ // MOLA virtual interfaces: #include #include +#include #include #include #include +#include // MRPT: #include @@ -37,6 +39,12 @@ #include #include +// MOLA <-> ROS services: +#include +#include +#include +#include + namespace mola { /** A bridge to read sensor observations from ROS 2 topics and forwarding them @@ -163,24 +171,18 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer auto lck = mrpt::lockHelper(rosNodeMtx_); return rosNode_; } - std::vector::SharedPtr> - subsPointCloud_; + std::vector::SharedPtr> subsPointCloud_; - std::vector::SharedPtr> - subsLaserScan_; + std::vector::SharedPtr> subsLaserScan_; - std::vector::SharedPtr> - subsOdometry_; + std::vector::SharedPtr> subsOdometry_; - std::vector::SharedPtr> - subsImu_; + std::vector::SharedPtr> subsImu_; - std::vector::SharedPtr> - subsGNSS_; + std::vector::SharedPtr> subsGNSS_; void callbackOnPointCloud2( - const sensor_msgs::msg::PointCloud2& o, - const std::string& outSensorLabel, + const sensor_msgs::msg::PointCloud2& o, const std::string& outSensorLabel, const std::optional& fixedSensorPose); void callbackOnLaserScan( @@ -195,12 +197,11 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer const sensor_msgs::msg::NavSatFix& o, const std::string& outSensorLabel, const std::optional& fixedSensorPose); - void callbackOnOdometry( - const nav_msgs::msg::Odometry& o, const std::string& outSensorLabel); + void callbackOnOdometry(const nav_msgs::msg::Odometry& o, const std::string& outSensorLabel); bool waitForTransform( - mrpt::poses::CPose3D& des, const std::string& target_frame, - const std::string& source_frame, bool printErrors); + mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame, + bool printErrors); void publishOdometry(); @@ -211,9 +212,7 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer struct RosPubs { /// Map => publisher - std::map< - std::string, rclcpp::Publisher::SharedPtr> - pub_poses; + std::map::SharedPtr> pub_poses; /// Map => publisher std::map pub_sensors; @@ -227,23 +226,46 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer struct MolaSubs { + // MOLA subscribers: std::set dataSources; std::set> locSources; std::set> mapSources; + std::set> relocalization; + std::set> mapServers; }; MolaSubs molaSubs_; std::mutex molaSubsMtx_; - void onNewLocalization( - const mola::LocalizationSourceBase::LocalizationUpdate& l); + // ROS services: + rclcpp::Service::SharedPtr srvRelocGNNS_; + rclcpp::Service::SharedPtr srvRelocPose_; + rclcpp::Service::SharedPtr srvMapLoad_; + rclcpp::Service::SharedPtr srvMapSave_; + + void service_relocalize_from_gnss( + const std::shared_ptr request, + std::shared_ptr response); + + void service_relocalize_near_pose( + const std::shared_ptr request, + std::shared_ptr response); + + void service_map_load( + const std::shared_ptr request, + std::shared_ptr response); + + void service_map_save( + const std::shared_ptr request, + std::shared_ptr response); + + void onNewLocalization(const mola::LocalizationSourceBase::LocalizationUpdate& l); void onNewMap(const mola::MapSourceBase::MapUpdate& m); - std::mutex lastLocMapMtx_; - std::optional lastLoc_; - std::map - lastMaps_; + std::mutex lastLocMapMtx_; + std::optional lastLoc_; + std::map lastMaps_; void timerPubLocalization(); void timerPubMap(); @@ -261,8 +283,7 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer const mrpt::obs::CObservationPointCloud& obs, bool isSensorTopic, const std::string& sSensorFrameId); - void internalAnalyzeTopicsToSubscribe( - const mrpt::containers::yaml& ds_subscribe); + void internalAnalyzeTopicsToSubscribe(const mrpt::containers::yaml& ds_subscribe); void publishStaticTFs(); }; diff --git a/mola_bridge_ros2/package.xml b/mola_bridge_ros2/package.xml index cda652b88b..37fa464cf0 100644 --- a/mola_bridge_ros2/package.xml +++ b/mola_bridge_ros2/package.xml @@ -5,16 +5,14 @@ --> mola_bridge_ros2 - 1.0.8 + 1.1.0 Bidirectional bridge ROS2-MOLA - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + BSD https://github.com/MOLAorg/mola/tree/develop/mola_bridge_ros2 - BSD - doxygen @@ -29,6 +27,7 @@ mola_common mola_kernel + mola_msgs mrpt2 @@ -39,12 +38,9 @@ tf2 tf2_geometry_msgs - ament_lint_auto - ament_lint_common - ament_lint_auto - ament_lint_common - ament_cmake_xmllint + ament_cmake_xmllint + ament_lint_cmake ament_cmake diff --git a/mola_bridge_ros2/src/BridgeROS2.cpp b/mola_bridge_ros2/src/BridgeROS2.cpp index 80d572689a..4390fe6cb4 100644 --- a/mola_bridge_ros2/src/BridgeROS2.cpp +++ b/mola_bridge_ros2/src/BridgeROS2.cpp @@ -97,16 +97,14 @@ void BridgeROS2::ros_node_thread_main(Yaml cfg) // TF buffer: tf_buffer_ = std::make_shared(); // ros_clock_ // tf_buffer_->setUsingDedicatedThread(true); - tf_listener_ = - std::make_shared(*tf_buffer_); + tf_listener_ = std::make_shared(*tf_buffer_); // TF broadcaster: tf_bc_ = std::make_shared(rosNode_); // It seems /tf does not find the connection between frames correctly if // using tf_static (!) - tf_static_bc_ = - std::make_shared(rosNode_); + tf_static_bc_ = std::make_shared(rosNode_); // Subscribe to topics as described by MOLA YAML parameters: auto ds_subscribe = cfg["subscribe"]; @@ -119,19 +117,19 @@ void BridgeROS2::ros_node_thread_main(Yaml cfg) else { internalAnalyzeTopicsToSubscribe(ds_subscribe); } auto timerLoc = rosNode_->create_wall_timer( - std::chrono::microseconds(static_cast( - 1e6 * params_.period_publish_new_localization)), + std::chrono::microseconds( + static_cast(1e6 * params_.period_publish_new_localization)), [this]() { timerPubLocalization(); }); auto timerMap = rosNode_->create_wall_timer( - std::chrono::microseconds(static_cast( - 1e6 * params_.period_publish_new_map)), + std::chrono::microseconds( + static_cast(1e6 * params_.period_publish_new_map)), [this]() { timerPubMap(); }); // Static tf: auto timerStaticTFs = rosNode_->create_wall_timer( - std::chrono::microseconds(static_cast( - 1e6 * params_.period_publish_static_tfs)), + std::chrono::microseconds( + static_cast(1e6 * params_.period_publish_static_tfs)), [this]() { publishStaticTFs(); }); // Spin: @@ -175,8 +173,7 @@ void BridgeROS2::initialize_rds(const Yaml& c) const auto s = cfg["base_footprint_to_base_link_tf"].as(); // Format: "[x y z yaw pitch roll]" (meters & degrees) - params_.base_footprint_to_base_link_tf = - mrpt::math::TPose3D::FromString(s); + params_.base_footprint_to_base_link_tf = mrpt::math::TPose3D::FromString(s); } // params of the MOLA-ROS2 part: @@ -189,8 +186,7 @@ void BridgeROS2::initialize_rds(const Yaml& c) YAML_LOAD_OPT(params_, publish_tf_from_robot_pose_observations, bool); // Launch ROS node: - rosNodeThread_ = - std::thread(&BridgeROS2::ros_node_thread_main, this, cfgCopy); + rosNodeThread_ = std::thread(&BridgeROS2::ros_node_thread_main, this, cfgCopy); MRPT_END } // end initialize() @@ -206,8 +202,7 @@ void BridgeROS2::spinOnce() publishOdometry(); // Check for new mola data sources? - if (mrpt::Clock::nowDouble() - lastTimeCheckMolaSubs_ > - params_.period_check_new_mola_subs) + if (mrpt::Clock::nowDouble() - lastTimeCheckMolaSubs_ > params_.period_check_new_mola_subs) { lastTimeCheckMolaSubs_ = mrpt::Clock::nowDouble(); doLookForNewMolaSubs(); @@ -227,8 +222,7 @@ void BridgeROS2::callbackOnPointCloud2( mrpt::maps::CPointsMap::Ptr mapPtr; - if (fields.count("time") || fields.count("timestamp") || - fields.count("ring")) + if (fields.count("time") || fields.count("timestamp") || fields.count("ring")) { auto p = mrpt::maps::CPointsMapXYZIRT::Create(); if (!mrpt::ros2bridge::fromROS(o, *p)) @@ -268,8 +262,7 @@ void BridgeROS2::callbackOnPointCloud2( { // Get pose from tf: bool ok = waitForTransform( - obs_pc->sensorPose, o.header.frame_id, params_.base_link_frame, - true /*print errors*/); + obs_pc->sensorPose, o.header.frame_id, params_.base_link_frame, true /*print errors*/); if (!ok) { @@ -289,8 +282,8 @@ void BridgeROS2::callbackOnPointCloud2( } bool BridgeROS2::waitForTransform( - mrpt::poses::CPose3D& des, const std::string& frame, - const std::string& referenceFrame, bool printErrors) + mrpt::poses::CPose3D& des, const std::string& frame, const std::string& referenceFrame, + bool printErrors) { try { @@ -302,8 +295,8 @@ bool BridgeROS2::waitForTransform( des = mrpt::ros2bridge::fromROS(tf); MRPT_LOG_DEBUG_FMT( - "[waitForTransform] Found pose %s -> %s: %s", - referenceFrame.c_str(), frame.c_str(), des.asString().c_str()); + "[waitForTransform] Found pose %s -> %s: %s", referenceFrame.c_str(), frame.c_str(), + des.asString().c_str()); return true; } @@ -323,8 +316,7 @@ void BridgeROS2::callbackOnOdometry( auto obs = mrpt::obs::CObservationOdometry::Create(); obs->timestamp = mrpt::ros2bridge::fromROS(o.header.stamp); obs->sensorLabel = outSensorLabel; - obs->odometry = - mrpt::poses::CPose2D(mrpt::ros2bridge::fromROS(o.pose.pose)); + obs->odometry = mrpt::poses::CPose2D(mrpt::ros2bridge::fromROS(o.pose.pose)); obs->hasVelocities = true; obs->velocityLocal.vx = o.twist.twist.linear.x; @@ -351,8 +343,7 @@ void BridgeROS2::publishOdometry() mrpt::poses::CPose3D odomPose; bool odom_tf_ok = waitForTransform( - odomPose, params_.base_link_frame, params_.odom_frame, - false /*dont print errors*/); + odomPose, params_.base_link_frame, params_.odom_frame, false /*dont print errors*/); if (!odom_tf_ok) { MRPT_LOG_THROTTLE_WARN_FMT( @@ -391,8 +382,7 @@ void BridgeROS2::callbackOnLaserScan( { // Get pose from tf: bool ok = waitForTransform( - sensorPose, o.header.frame_id, params_.base_link_frame, - true /*print errors*/); + sensorPose, o.header.frame_id, params_.base_link_frame, true /*print errors*/); if (!ok) { @@ -434,8 +424,7 @@ void BridgeROS2::callbackOnImu( { // Get pose from tf: bool ok = waitForTransform( - sensorPose, o.header.frame_id, params_.base_link_frame, - true /*print errors*/); + sensorPose, o.header.frame_id, params_.base_link_frame, true /*print errors*/); if (!ok) { MRPT_LOG_ERROR_FMT( @@ -477,8 +466,7 @@ void BridgeROS2::callbackOnNavSatFix( { // Get pose from tf: bool ok = waitForTransform( - sensorPose, o.header.frame_id, params_.base_link_frame, - true /*print errors*/); + sensorPose, o.header.frame_id, params_.base_link_frame, true /*print errors*/); if (!ok) { MRPT_LOG_ERROR_FMT( @@ -514,18 +502,15 @@ void BridgeROS2::onNewObservation(const CObservation::Ptr& o) { return internalOn(*oImg); } - else if (auto oPc = std::dynamic_pointer_cast(o); - oPc) + else if (auto oPc = std::dynamic_pointer_cast(o); oPc) { return internalOn(*oPc); } - else if (auto oLS = std::dynamic_pointer_cast(o); - oLS) + else if (auto oLS = std::dynamic_pointer_cast(o); oLS) { return internalOn(*oLS); } - else if (auto oRP = std::dynamic_pointer_cast(o); - oRP) + else if (auto oRP = std::dynamic_pointer_cast(o); oRP) { return internalOn(*oRP); } @@ -548,8 +533,8 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationImage& 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(); + 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) @@ -562,8 +547,7 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationImage& obs) lck.unlock(); rclcpp::Publisher::SharedPtr pubImg = - std::dynamic_pointer_cast>( - pub); + std::dynamic_pointer_cast>(pub); ASSERT_(pubImg); const std::string sSensorFrameId = obs.sensorLabel; @@ -602,8 +586,8 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservation2DRangeScan& 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(); + 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) @@ -616,8 +600,7 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservation2DRangeScan& obs) lck.unlock(); rclcpp::Publisher::SharedPtr pubLS = - std::dynamic_pointer_cast< - rclcpp::Publisher>(pub); + std::dynamic_pointer_cast>(pub); ASSERT_(pubLS); const std::string sSensorFrameId = obs.sensorLabel; @@ -652,9 +635,7 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservation2DRangeScan& obs) void BridgeROS2::internalOn(const mrpt::obs::CObservationPointCloud& obs) { - internalOn( - obs, true /*it is a real sensor, publish its /tf*/, - obs.sensorLabel /* frame_id */); + internalOn(obs, true /*it is a real sensor, publish its /tf*/, obs.sensorLabel /* frame_id */); } void BridgeROS2::internalOn( @@ -668,8 +649,7 @@ void BridgeROS2::internalOn( const auto lbPoints = obs.sensorLabel + "_points"s; // Create the publisher the first time an observation arrives: - const bool is_1st_pub = - rosPubs_.pub_sensors.find(lbPoints) == rosPubs_.pub_sensors.end(); + const bool is_1st_pub = rosPubs_.pub_sensors.find(lbPoints) == rosPubs_.pub_sensors.end(); auto& pubPts = rosPubs_.pub_sensors[lbPoints]; @@ -678,19 +658,15 @@ void BridgeROS2::internalOn( // REP-2003: https://ros.org/reps/rep-2003.html#id5 // - Sensors: SystemDefaultsQoS() // - Maps: reliable transient-local - auto qos = - isSensorTopic - ? rclcpp::SystemDefaultsQoS() - : rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + auto qos = isSensorTopic ? rclcpp::SystemDefaultsQoS() + : rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); - pubPts = rosNode()->create_publisher( - lbPoints, qos); + pubPts = rosNode()->create_publisher(lbPoints, qos); } lck.unlock(); rclcpp::Publisher::SharedPtr pubPoints = - std::dynamic_pointer_cast< - rclcpp::Publisher>(pubPts); + std::dynamic_pointer_cast>(pubPts); ASSERT_(pubPoints); const std::string sSensorFrameId_points = lbPoints; @@ -703,8 +679,7 @@ void BridgeROS2::internalOn( { mrpt::poses::CPose3D sensorPose = obs.sensorPose; - tf2::Transform transform = - mrpt::ros2bridge::toROS_tfTransform(sensorPose); + tf2::Transform transform = mrpt::ros2bridge::toROS_tfTransform(sensorPose); geometry_msgs::msg::TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -725,20 +700,18 @@ void BridgeROS2::internalOn( obs.load(); - if (auto* xyzirt = dynamic_cast( - obs.pointcloud.get()); + if (auto* xyzirt = dynamic_cast(obs.pointcloud.get()); xyzirt) { mrpt::ros2bridge::toROS(*xyzirt, msg_header, msg_pts); } - else if (auto* xyzi = dynamic_cast( - obs.pointcloud.get()); + else if (auto* xyzi = dynamic_cast(obs.pointcloud.get()); xyzi) { mrpt::ros2bridge::toROS(*xyzi, msg_header, msg_pts); } - else if (auto* sPts = dynamic_cast( - obs.pointcloud.get()); + else if (auto* sPts = + dynamic_cast(obs.pointcloud.get()); sPts) { mrpt::ros2bridge::toROS(*sPts, msg_header, msg_pts); @@ -762,8 +735,8 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationRobotPose& obs) ASSERT_(!obs.sensorLabel.empty()); // Create the publisher the first time an observation arrives: - const bool is_1st_pub = rosPubs_.pub_sensors.find(obs.sensorLabel) == - rosPubs_.pub_sensors.end(); + 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) @@ -776,15 +749,13 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationRobotPose& obs) lck.unlock(); rclcpp::Publisher::SharedPtr pubOdo = - std::dynamic_pointer_cast>( - pub); + std::dynamic_pointer_cast>(pub); ASSERT_(pubOdo); // Send TF: if (params_.publish_tf_from_robot_pose_observations) { - tf2::Transform transform = - mrpt::ros2bridge::toROS_tfTransform(obs.pose.mean); + tf2::Transform transform = mrpt::ros2bridge::toROS_tfTransform(obs.pose.mean); geometry_msgs::msg::TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -814,8 +785,8 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationGPS& 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(); + 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) @@ -828,8 +799,7 @@ void BridgeROS2::internalOn(const mrpt::obs::CObservationGPS& obs) lck.unlock(); rclcpp::Publisher::SharedPtr pubGPS = - std::dynamic_pointer_cast< - rclcpp::Publisher>(pub); + std::dynamic_pointer_cast>(pub); ASSERT_(pubGPS); const std::string sSensorFrameId = obs.sensorLabel; @@ -877,15 +847,12 @@ void BridgeROS2::doLookForNewMolaSubs() ASSERT_(rds); // Skip myself! - if (std::string(rds->GetRuntimeClass()->className) == - "mola::BridgeROS2"s) - continue; + if (std::string(rds->GetRuntimeClass()->className) == "mola::BridgeROS2"s) continue; if (molaSubs_.dataSources.count(rds) == 0) { MRPT_LOG_INFO_STREAM( - "Subscribing to MOLA data source module '" - << rds->getModuleInstanceName() << "'"); + "Subscribing to MOLA data source module '" << rds->getModuleInstanceName() << "'"); // a new one: molaSubs_.dataSources.insert(rds); @@ -897,8 +864,7 @@ void BridgeROS2::doLookForNewMolaSubs() auto listLoc = this->findService(); for (auto& module : listLoc) { - auto loc = - std::dynamic_pointer_cast(module); + auto loc = std::dynamic_pointer_cast(module); ASSERT_(loc); if (molaSubs_.locSources.count(loc) == 0) @@ -909,8 +875,7 @@ void BridgeROS2::doLookForNewMolaSubs() // a new one: molaSubs_.locSources.insert(loc); - loc->subscribeToLocalizationUpdates([this](const auto& l) - { onNewLocalization(l); }); + loc->subscribeToLocalizationUpdates([this](const auto& l) { onNewLocalization(l); }); } } @@ -924,14 +889,160 @@ void BridgeROS2::doLookForNewMolaSubs() if (molaSubs_.mapSources.count(ms) == 0) { MRPT_LOG_INFO_STREAM( - "Subscribing to MOLA map source module '" - << module->getModuleInstanceName() << "'"); + "Subscribing to MOLA map source module '" << module->getModuleInstanceName() + << "'"); // a new one: molaSubs_.mapSources.insert(ms); ms->subscribeToMapUpdates([this](const auto& m) { onNewMap(m); }); } } + + // relocalization + auto listRelog = this->findService(); + for (auto& module : listRelog) + { + auto ms = std::dynamic_pointer_cast(module); + ASSERT_(ms); + + if (molaSubs_.relocalization.count(ms) == 0) + { + MRPT_LOG_INFO_STREAM( + "Subscribing to MOLA relocalization module '" << module->getModuleInstanceName() + << "'"); + + // a new one: + molaSubs_.relocalization.insert(ms); + } + } + + // Advertise relocalization ROS 2 service now if not done already: + if (!molaSubs_.relocalization.empty() && !srvRelocGNNS_) + { + using namespace std::placeholders; + + srvRelocGNNS_ = rosNode_->create_service( + "relocalize_from_gnss", + std::bind(&BridgeROS2::service_relocalize_from_gnss, this, _1, _2)); + + srvRelocPose_ = rosNode_->create_service( + "relocalize_near_pose", + std::bind(&BridgeROS2::service_relocalize_near_pose, this, _1, _2)); + } + + // Map server + auto listMapServers = this->findService(); + for (auto& module : listMapServers) + { + auto ms = std::dynamic_pointer_cast(module); + ASSERT_(ms); + + if (molaSubs_.mapServers.count(ms) == 0) + { + MRPT_LOG_INFO_STREAM( + "Subscribing to MOLA map server module '" << module->getModuleInstanceName() + << "'"); + + if (molaSubs_.mapServers.size() > 1) + { + MRPT_LOG_WARN( + "More than one MapServer MOLA modules seems to be running. ROS 2 requests will " + "be forwarded to the first module only."); + } + + // a new one: + molaSubs_.mapServers.insert(ms); + } + } + + // Advertise map server ROS 2 services now if not done already: + if (!molaSubs_.mapServers.empty() && !srvMapLoad_) + { + using namespace std::placeholders; + + srvMapLoad_ = rosNode_->create_service( + "map_load", std::bind(&BridgeROS2::service_map_load, this, _1, _2)); + + srvMapSave_ = rosNode_->create_service( + "map_save", std::bind(&BridgeROS2::service_map_save, this, _1, _2)); + } +} + +void BridgeROS2::service_relocalize_from_gnss( + [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response) +{ + auto lck = mrpt::lockHelper(rosPubsMtx_); + if (molaSubs_.relocalization.empty()) + { + response->accepted = false; + return; + } + + for (auto m : molaSubs_.relocalization) { m->relocalize_from_gnss(); } + response->accepted = true; +} + +void BridgeROS2::service_relocalize_near_pose( + const std::shared_ptr request, + std::shared_ptr response) +{ + auto lck = mrpt::lockHelper(rosPubsMtx_); + if (molaSubs_.relocalization.empty()) + { + response->accepted = false; + return; + } + + for (auto m : molaSubs_.relocalization) + { + const mrpt::poses::CPose3DPDFGaussian p = mrpt::ros2bridge::fromROS(request->pose.pose); + m->relocalize_near_pose_pdf(p); + } + + response->accepted = true; +} + +void BridgeROS2::service_map_load( + const std::shared_ptr request, + std::shared_ptr response) +{ + auto lck = mrpt::lockHelper(rosPubsMtx_); + if (molaSubs_.relocalization.empty()) + { + response->success = false; + response->error_message = "No MOLA module with MapServer interface is running."; + MRPT_LOG_WARN(response->error_message); + return; + } + + auto& m = *molaSubs_.mapServers.begin(); + ASSERT_(m); + const auto& r = m->map_load(request->map_path); + + response->success = r.success; + response->error_message = r.error_message; +} + +void BridgeROS2::service_map_save( + const std::shared_ptr request, + std::shared_ptr response) +{ + auto lck = mrpt::lockHelper(rosPubsMtx_); + if (molaSubs_.relocalization.empty()) + { + response->success = false; + response->error_message = "No MOLA module with MapServer interface is running."; + MRPT_LOG_WARN(response->error_message); + return; + } + + auto& m = *molaSubs_.mapServers.begin(); + ASSERT_(m); + const auto& r = m->map_save(request->map_path); + + response->success = r.success; + response->error_message = r.error_message; } rclcpp::Time BridgeROS2::myNow(const mrpt::Clock::time_point& observationStamp) @@ -942,8 +1053,7 @@ rclcpp::Time BridgeROS2::myNow(const mrpt::Clock::time_point& observationStamp) return mrpt::ros2bridge::toROS(mrpt::Clock::now()); } -void BridgeROS2::onNewLocalization( - const mola::LocalizationSourceBase::LocalizationUpdate& l) +void BridgeROS2::onNewLocalization(const mola::LocalizationSourceBase::LocalizationUpdate& l) { auto lck = mrpt::lockHelper(lastLocMapMtx_); @@ -972,20 +1082,17 @@ void BridgeROS2::timerPubLocalization() MRPT_LOG_DEBUG_STREAM( "New localization available from '" - << l->method << "' frame: '" << l->reference_frame - << "' t=" << mrpt::system::dateTimeLocalToString(l->timestamp) - << " pose=" << l->pose.asString()); + << l->method << "' frame: '" << l->reference_frame << "' t=" + << mrpt::system::dateTimeLocalToString(l->timestamp) << " pose=" << l->pose.asString()); // 1/2: Publish to /tf: - const std::string locLabel = - (l->method.empty() ? "slam"s : l->method) + "/pose"s; + const std::string locLabel = (l->method.empty() ? "slam"s : l->method) + "/pose"s; auto lck = mrpt::lockHelper(rosPubsMtx_); // Create the publisher the first time an observation arrives: - const bool is_1st_pub = - rosPubs_.pub_sensors.find(locLabel) == rosPubs_.pub_sensors.end(); - auto& pub = rosPubs_.pub_sensors[locLabel]; + const bool is_1st_pub = rosPubs_.pub_sensors.find(locLabel) == rosPubs_.pub_sensors.end(); + auto& pub = rosPubs_.pub_sensors[locLabel]; if (is_1st_pub) { @@ -995,8 +1102,7 @@ void BridgeROS2::timerPubLocalization() lck.unlock(); rclcpp::Publisher::SharedPtr pubOdo = - std::dynamic_pointer_cast>( - pub); + std::dynamic_pointer_cast>(pub); ASSERT_(pubOdo); // Send TF: @@ -1044,20 +1150,17 @@ void BridgeROS2::timerPubMap() for (const auto& [layerName, mu] : m) { - const std::string mapTopic = - (mu.method.empty() ? "slam"s : mu.method) + "/"s + layerName; + const std::string mapTopic = (mu.method.empty() ? "slam"s : mu.method) + "/"s + layerName; // Reuse code for point cloud observations: build a "fake" observation: mrpt::obs::CObservationPointCloud obs; obs.sensorLabel = mapTopic; - obs.pointcloud = - std::dynamic_pointer_cast(mu.map); + obs.pointcloud = std::dynamic_pointer_cast(mu.map); if (!obs.pointcloud) { MRPT_LOG_WARN_STREAM( "Do not know how to publish map layer '" - << layerName << "' of type '" - << mu.map->GetRuntimeClass()->className << "'"); + << layerName << "' of type '" << mu.map->GetRuntimeClass()->className << "'"); continue; } @@ -1065,8 +1168,7 @@ void BridgeROS2::timerPubMap() } } -void BridgeROS2::internalAnalyzeTopicsToSubscribe( - const mrpt::containers::yaml& ds_subscribe) +void BridgeROS2::internalAnalyzeTopicsToSubscribe(const mrpt::containers::yaml& ds_subscribe) { using namespace std::string_literals; @@ -1082,20 +1184,17 @@ void BridgeROS2::internalAnalyzeTopicsToSubscribe( ENSURE_YAML_ENTRY_EXISTS(topic, "msg_type"); ENSURE_YAML_ENTRY_EXISTS(topic, "output_sensor_label"); - const auto topic_name = topic["topic"].as(); - const auto type = topic["msg_type"].as(); - const auto output_sensor_label = - topic["output_sensor_label"].as(); + const auto topic_name = topic["topic"].as(); + const auto type = topic["msg_type"].as(); + const auto output_sensor_label = topic["output_sensor_label"].as(); MRPT_LOG_DEBUG_STREAM( - "Creating ros2 subscriber for topic='" << topic_name << "' (" - << type << ")"); + "Creating ros2 subscriber for topic='" << topic_name << "' (" << type << ")"); // Optional: fixed sensorPose (then ignores/don't need "tf" data): std::optional fixedSensorPose; if (topic.has("fixed_sensor_pose") && - (!topic.has("use_fixed_sensor_pose") || - topic["use_fixed_sensor_pose"].as())) + (!topic.has("use_fixed_sensor_pose") || topic["use_fixed_sensor_pose"].as())) { fixedSensorPose = mrpt::poses::CPose3D::FromString( "["s + topic["fixed_sensor_pose"].as() + "]"s); @@ -1107,58 +1206,41 @@ void BridgeROS2::internalAnalyzeTopicsToSubscribe( rosNode_->create_subscription( topic_name, qos, [this, output_sensor_label, - fixedSensorPose](const sensor_msgs::msg::PointCloud2& o) { - this->callbackOnPointCloud2( - o, output_sensor_label, fixedSensorPose); - })); + fixedSensorPose](const sensor_msgs::msg::PointCloud2& o) + { this->callbackOnPointCloud2(o, output_sensor_label, fixedSensorPose); })); } else if (type == "LaserScan") { - subsLaserScan_.emplace_back( - rosNode_->create_subscription( - topic_name, qos, - [this, output_sensor_label, - fixedSensorPose](const sensor_msgs::msg::LaserScan& o) { - this->callbackOnLaserScan( - o, output_sensor_label, fixedSensorPose); - })); + subsLaserScan_.emplace_back(rosNode_->create_subscription( + topic_name, qos, + [this, output_sensor_label, fixedSensorPose](const sensor_msgs::msg::LaserScan& o) + { this->callbackOnLaserScan(o, output_sensor_label, fixedSensorPose); })); } else if (type == "Imu") { - subsImu_.emplace_back( - rosNode_->create_subscription( - topic_name, qos, - [this, output_sensor_label, - fixedSensorPose](const sensor_msgs::msg::Imu& o) { - this->callbackOnImu( - o, output_sensor_label, fixedSensorPose); - })); + subsImu_.emplace_back(rosNode_->create_subscription( + topic_name, qos, + [this, output_sensor_label, fixedSensorPose](const sensor_msgs::msg::Imu& o) + { this->callbackOnImu(o, output_sensor_label, fixedSensorPose); })); } else if (type == "NavSatFix") { - subsGNSS_.emplace_back( - rosNode_->create_subscription( - topic_name, qos, - [this, output_sensor_label, - fixedSensorPose](const sensor_msgs::msg::NavSatFix& o) { - this->callbackOnNavSatFix( - o, output_sensor_label, fixedSensorPose); - })); + subsGNSS_.emplace_back(rosNode_->create_subscription( + topic_name, qos, + [this, output_sensor_label, fixedSensorPose](const sensor_msgs::msg::NavSatFix& o) + { this->callbackOnNavSatFix(o, output_sensor_label, fixedSensorPose); })); } else if (type == "Odometry") { - subsOdometry_.emplace_back( - rosNode_->create_subscription( - topic_name, qos, - [this, - output_sensor_label](const nav_msgs::msg::Odometry& o) - { this->callbackOnOdometry(o, output_sensor_label); })); + subsOdometry_.emplace_back(rosNode_->create_subscription( + topic_name, qos, + [this, output_sensor_label](const nav_msgs::msg::Odometry& o) + { this->callbackOnOdometry(o, output_sensor_label); })); } else { THROW_EXCEPTION_FMT( - "Unhandled type=`%s` for topic=`%s`", type.c_str(), - topic_name.c_str()); + "Unhandled type=`%s` for topic=`%s`", type.c_str(), topic_name.c_str()); } } } @@ -1167,8 +1249,8 @@ void BridgeROS2::publishStaticTFs() { if (!params_.base_footprint_frame.empty()) { - const tf2::Transform transform = mrpt::ros2bridge::toROS_tfTransform( - params_.base_footprint_to_base_link_tf); + const tf2::Transform transform = + mrpt::ros2bridge::toROS_tfTransform(params_.base_footprint_to_base_link_tf); geometry_msgs::msg::TransformStamped tfStmp; diff --git a/mola_demos/CHANGELOG.rst b/mola_demos/CHANGELOG.rst index e37070e3d9..c64189841b 100644 --- a/mola_demos/CHANGELOG.rst +++ b/mola_demos/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package mola_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_demos/package.xml b/mola_demos/package.xml index 120536b52f..0cfbdcb0af 100644 --- a/mola_demos/package.xml +++ b/mola_demos/package.xml @@ -5,17 +5,16 @@ --> mola_demos - 1.0.8 + 1.1.0 Demo and example launch files for MOLA - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + BSD https://github.com/MOLAorg/ - BSD - + doxygen diff --git a/mola_imu_preintegration/CHANGELOG.rst b/mola_imu_preintegration/CHANGELOG.rst index 1d03f86318..fc6390b787 100644 --- a/mola_imu_preintegration/CHANGELOG.rst +++ b/mola_imu_preintegration/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_imu_preintegration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_imu_preintegration/package.xml b/mola_imu_preintegration/package.xml index dc4e342904..fca1b92f13 100644 --- a/mola_imu_preintegration/package.xml +++ b/mola_imu_preintegration/package.xml @@ -5,15 +5,14 @@ --> mola_imu_preintegration - 1.0.8 + 1.1.0 Integrator of IMU angular velocity readings - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_imu_preintegration - GPLv3 mola_common mrpt2 diff --git a/mola_imu_preintegration/tests/test-imu-rotation-integrator.cpp b/mola_imu_preintegration/tests/test-imu-rotation-integrator.cpp index 9a755a620e..851df100ff 100644 --- a/mola_imu_preintegration/tests/test-imu-rotation-integrator.cpp +++ b/mola_imu_preintegration/tests/test-imu-rotation-integrator.cpp @@ -55,7 +55,8 @@ static void test_rotation_integration() // gtPose).norm(),1e-6); // Initial state: - auto lambdaAssertInitialState = [&ri]() { + auto lambdaAssertInitialState = [&ri]() + { const auto& s = ri.current_integration_state(); ASSERT_LT_( (s.deltaRij_ - mrpt::math::CMatrixDouble33::Identity()).norm(), diff --git a/mola_input_euroc_dataset/CHANGELOG.rst b/mola_input_euroc_dataset/CHANGELOG.rst index 224e5975e4..a82a1976ed 100644 --- a/mola_input_euroc_dataset/CHANGELOG.rst +++ b/mola_input_euroc_dataset/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_input_euroc_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_input_euroc_dataset/package.xml b/mola_input_euroc_dataset/package.xml index 86a7973b53..367439a2ba 100644 --- a/mola_input_euroc_dataset/package.xml +++ b/mola_input_euroc_dataset/package.xml @@ -5,15 +5,14 @@ --> mola_input_euroc_dataset - 1.0.8 + 1.1.0 Offline RawDataSource from EUROC SLAM datasets - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + BSD https://github.com/MOLAorg/mola/tree/develop/mola_input_euroc_dataset - BSD mola_common mola_kernel diff --git a/mola_input_euroc_dataset/src/EurocDataset.cpp b/mola_input_euroc_dataset/src/EurocDataset.cpp index e7d8f7c9a7..8441a8c57f 100644 --- a/mola_input_euroc_dataset/src/EurocDataset.cpp +++ b/mola_input_euroc_dataset/src/EurocDataset.cpp @@ -290,16 +290,17 @@ void EurocDataset::spinOnce() std::visit( overloaded{ - [&](std::monostate&) { - THROW_EXCEPTION("Un-initialized entry!"); - }, - [&](SensorCamera& cam) { + [&](std::monostate&) + { THROW_EXCEPTION("Un-initialized entry!"); }, + [&](SensorCamera& cam) + { build_dataset_entry_obs(cam); cam.obs->timestamp = obs_tim; this->sendObservationsToFrontEnds(cam.obs); cam.obs.reset(); // free mem }, - [&](SensorIMU& imu) { + [&](SensorIMU& imu) + { build_dataset_entry_obs(imu); imu.obs->timestamp = obs_tim; this->sendObservationsToFrontEnds(imu.obs); @@ -330,9 +331,8 @@ void EurocDataset::spinOnce() // std::visit( overloaded{ - [&](std::monostate&) { - THROW_EXCEPTION("Un-initialized entry!"); - }, + [&](std::monostate&) + { THROW_EXCEPTION("Un-initialized entry!"); }, [&](SensorCamera& cam) { build_dataset_entry_obs(cam); }, [&](SensorIMU& imu) { build_dataset_entry_obs(imu); }}, peeker->second); diff --git a/mola_input_kitti360_dataset/CHANGELOG.rst b/mola_input_kitti360_dataset/CHANGELOG.rst index 19d1c8b2d4..dd30539cd3 100644 --- a/mola_input_kitti360_dataset/CHANGELOG.rst +++ b/mola_input_kitti360_dataset/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package mola_input_kitti_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_input_kitti360_dataset/package.xml b/mola_input_kitti360_dataset/package.xml index 01450b3ed1..ddb507d843 100644 --- a/mola_input_kitti360_dataset/package.xml +++ b/mola_input_kitti360_dataset/package.xml @@ -5,15 +5,14 @@ --> mola_input_kitti360_dataset - 1.0.8 + 1.1.0 Offline RawDataSource from Kitti-360 datasets - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_input_kitti360_dataset - GPLv3 mola_common mola_kernel diff --git a/mola_input_kitti_dataset/CHANGELOG.rst b/mola_input_kitti_dataset/CHANGELOG.rst index aadd8cb4c6..c5d3d62706 100644 --- a/mola_input_kitti_dataset/CHANGELOG.rst +++ b/mola_input_kitti_dataset/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package mola_input_kitti_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_input_kitti_dataset/package.xml b/mola_input_kitti_dataset/package.xml index 5c15b8ab37..3a2a922ba0 100644 --- a/mola_input_kitti_dataset/package.xml +++ b/mola_input_kitti_dataset/package.xml @@ -5,15 +5,14 @@ --> mola_input_kitti_dataset - 1.0.8 + 1.1.0 Offline RawDataSource from Kitti odometry/SLAM datasets - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_input_kitti_dataset - GPLv3 mola_common mola_kernel diff --git a/mola_input_mulran_dataset/CHANGELOG.rst b/mola_input_mulran_dataset/CHANGELOG.rst index e56a2905f1..f4e38c63ca 100644 --- a/mola_input_mulran_dataset/CHANGELOG.rst +++ b/mola_input_mulran_dataset/CHANGELOG.rst @@ -3,6 +3,14 @@ Changelog for package mola_input_mulran_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_input_mulran_dataset/package.xml b/mola_input_mulran_dataset/package.xml index 25886229d0..66f5f13e91 100644 --- a/mola_input_mulran_dataset/package.xml +++ b/mola_input_mulran_dataset/package.xml @@ -5,15 +5,14 @@ --> mola_input_mulran_dataset - 1.0.8 + 1.1.0 Offline RawDataSource from MulRan datasets - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_input_mulran_dataset - GPLv3 mola_common mola_kernel diff --git a/mola_input_mulran_dataset/src/MulranDataset.cpp b/mola_input_mulran_dataset/src/MulranDataset.cpp index dbf5a08934..07e5d8a512 100644 --- a/mola_input_mulran_dataset/src/MulranDataset.cpp +++ b/mola_input_mulran_dataset/src/MulranDataset.cpp @@ -468,7 +468,7 @@ void MulranDataset::load_lidar(timestep_t step) const // Pose: obs->sensorPose = ousterPoseOnVehicle_; obs->timestamp = mrpt::Clock::fromDouble( - LidarFileNameToTimestamp(lstPointCloudFiles_[step])); + LidarFileNameToTimestamp(lstPointCloudFiles_[step])); #if 0 // Export clouds to txt for debugging externally (e.g. python, matlab) pts->saveXYZIRT_to_text_file( diff --git a/mola_input_paris_luco_dataset/CHANGELOG.rst b/mola_input_paris_luco_dataset/CHANGELOG.rst index 95bd3ea61f..94464178d3 100644 --- a/mola_input_paris_luco_dataset/CHANGELOG.rst +++ b/mola_input_paris_luco_dataset/CHANGELOG.rst @@ -3,6 +3,14 @@ Changelog for package mola_input_paris_luco_dataset ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_input_paris_luco_dataset/package.xml b/mola_input_paris_luco_dataset/package.xml index 0152885203..b42279ac85 100644 --- a/mola_input_paris_luco_dataset/package.xml +++ b/mola_input_paris_luco_dataset/package.xml @@ -5,15 +5,14 @@ --> mola_input_paris_luco_dataset - 1.0.8 + 1.1.0 Offline RawDataSource from Paris LUCO (CT-ICP) odometry/SLAM datasets - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_input_paris_luco_dataset - GPLv3 mola_common mola_kernel diff --git a/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp b/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp index b8a47c4b46..73d88d1627 100644 --- a/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp +++ b/mola_input_paris_luco_dataset/src/ParisLucoDataset.cpp @@ -274,9 +274,9 @@ void ParisLucoDataset::load_lidar(timestep_t step) const const float earliestTime = *std::min_element(Ts->cbegin(), Ts->cend()); const float shiftTime = -earliestTime - 0.5 * lidarPeriod_; - std::transform(Ts->cbegin(), Ts->cend(), Ts->begin(), [=](double t) { - return t + shiftTime; - }); + std::transform( + Ts->cbegin(), Ts->cend(), Ts->begin(), + [=](double t) { return t + shiftTime; }); // Fix missing RING_ID: ParisLuco does not have a RING_ID field, // but we can generate it from the timestamps + pitch angle: diff --git a/mola_input_rawlog/CHANGELOG.rst b/mola_input_rawlog/CHANGELOG.rst index c8193bd715..6538a26604 100644 --- a/mola_input_rawlog/CHANGELOG.rst +++ b/mola_input_rawlog/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_input_rawlog ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_input_rawlog/package.xml b/mola_input_rawlog/package.xml index d0da4f1a0f..8b45621cb9 100644 --- a/mola_input_rawlog/package.xml +++ b/mola_input_rawlog/package.xml @@ -5,15 +5,14 @@ --> mola_input_rawlog - 1.0.8 + 1.1.0 Offline RawDataSource from MRPT rawlog datasets - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_input_rawlog - GPLv3 mola_kernel mrpt2 diff --git a/mola_input_rawlog/src/RawlogDataset.cpp b/mola_input_rawlog/src/RawlogDataset.cpp index 7ca160f4ad..3919a5655d 100644 --- a/mola_input_rawlog/src/RawlogDataset.cpp +++ b/mola_input_rawlog/src/RawlogDataset.cpp @@ -202,22 +202,23 @@ void RawlogDataset::doReadAheadFromFile() std::dynamic_pointer_cast( obj); sf) - { - for (const auto& o : *sf) - read_ahead_.emplace(o->getTimeStamp(), o); - } - else if (auto acts = std::dynamic_pointer_cast< - mrpt::obs::CActionCollection>(obj); - acts) - { - // odometry actions: ignore - } - else - THROW_EXCEPTION_FMT( - "Rawlog file can contain classes: " - "CObservation|CSensoryFrame|CActionCollection, but class " - "'%s' found.", - obj->GetRuntimeClass()->className); + { + for (const auto& o : *sf) + read_ahead_.emplace(o->getTimeStamp(), o); + } + else if (auto acts = std::dynamic_pointer_cast< + mrpt::obs::CActionCollection>(obj); + acts) + { + // odometry actions: ignore + } + else + THROW_EXCEPTION_FMT( + "Rawlog file can contain classes: " + "CObservation|CSensoryFrame|CActionCollection, but " + "class " + "'%s' found.", + obj->GetRuntimeClass()->className); } catch (const mrpt::serialization::CExceptionEOF&) { @@ -247,22 +248,22 @@ void RawlogDataset::doReadAheadFromEntireRawlog() if (auto sf = std::dynamic_pointer_cast(obj); sf) - { - for (const auto& o : *sf) read_ahead_.emplace(o->getTimeStamp(), o); - } - else if (auto acts = - std::dynamic_pointer_cast( - obj); - acts) - { - // odometry actions: ignore - } - else - THROW_EXCEPTION_FMT( - "Rawlog file can contain classes: " - "CObservation|CSensoryFrame|CActionCollection, but class " - "'%s' found.", - obj->GetRuntimeClass()->className); + { + for (const auto& o : *sf) + read_ahead_.emplace(o->getTimeStamp(), o); + } + else if (auto acts = std::dynamic_pointer_cast< + mrpt::obs::CActionCollection>(obj); + acts) + { + // odometry actions: ignore + } + else + THROW_EXCEPTION_FMT( + "Rawlog file can contain classes: " + "CObservation|CSensoryFrame|CActionCollection, but class " + "'%s' found.", + obj->GetRuntimeClass()->className); } } @@ -331,17 +332,18 @@ mrpt::obs::CSensoryFrame::Ptr RawlogDataset::datasetGetObservations( else // if (auto sf = std::dynamic_pointer_cast(obj); sf) - { - sfRet = sf; + { + sfRet = sf; - for (auto& o : *sf) unload_queue_.emplace(o->getTimeStamp(), o); - } - else if (auto acts = - std::dynamic_pointer_cast(obj); - acts) - { - // odometry actions: ignore - } + for (auto& o : *sf) unload_queue_.emplace(o->getTimeStamp(), o); + } + else if (auto acts = + std::dynamic_pointer_cast( + obj); + acts) + { + // odometry actions: ignore + } return sfRet; } diff --git a/mola_input_rosbag2/CHANGELOG.rst b/mola_input_rosbag2/CHANGELOG.rst index 861ec5ec25..b029fde8f7 100644 --- a/mola_input_rosbag2/CHANGELOG.rst +++ b/mola_input_rosbag2/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package mola_input_rosbag2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_input_rosbag2/package.xml b/mola_input_rosbag2/package.xml index f9d41db408..4e49a6e3e5 100644 --- a/mola_input_rosbag2/package.xml +++ b/mola_input_rosbag2/package.xml @@ -5,15 +5,14 @@ --> mola_input_rosbag2 - 1.0.8 + 1.1.0 Offline RawDataSource from rosbag2 datasets - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_input_rosbag2 - GPLv3 mola_kernel mrpt2 diff --git a/mola_kernel/CHANGELOG.rst b/mola_kernel/CHANGELOG.rst index 472fc5e9c4..8f9417a205 100644 --- a/mola_kernel/CHANGELOG.rst +++ b/mola_kernel/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package mola_kernel ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* add with a version-checking macro +* Merge pull request `#65 `_ from MOLAorg/add-more-srvs + Add more Services +* Avoid cmake file glob expressions +* mola_kernel: add MapServer interface +* mola_kernel: add public symbols MOLA\_{MAJOR,MINOR,PATCH}_VERSION +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * mola_kernel: add C++ virtual interface for relocalization methods diff --git a/mola_kernel/CMakeLists.txt b/mola_kernel/CMakeLists.txt index 21141b979e..ef54779052 100644 --- a/mola_kernel/CMakeLists.txt +++ b/mola_kernel/CMakeLists.txt @@ -21,8 +21,77 @@ find_package(mola_yaml REQUIRED) find_package(MRPT 2.1.0 REQUIRED COMPONENTS gui obs maps) # define lib: -file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h) -file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h) +set(LIB_SRCS + src/factors/FactorBase.cpp + src/factors/FactorRelativePose3.cpp + src/factors/SmartFactorIMU.cpp + src/factors/SmartFactorStereoProjectionPose.cpp + src/factors/FactorConstVelKinematics.cpp + src/factors/FactorStereoProjectionPose.cpp + src/register.cpp + src/WorldModel.cpp + src/interfaces/NavStateFilter.cpp + src/interfaces/RawDataSourceBase.cpp + src/interfaces/Dataset_UI.cpp + src/interfaces/FrontEndBase.cpp + src/interfaces/BackEndBase.cpp + src/interfaces/FilterBase.cpp + src/interfaces/ExecutableBase.cpp + src/interfaces/Relocalization.cpp + src/interfaces/MapServer.cpp + src/Entity.cpp + src/Factor.cpp + src/entities/RelPose3.cpp + src/entities/RefPose3.cpp + src/entities/RelDynPose3KF.cpp + src/entities/LandmarkPoint3.cpp + src/entities/EntityBase.cpp + src/entities/RelPose3KF.cpp + src/pretty_print_exception.cpp + src/LazyLoadResource.cpp +) + +set(LIB_PUBLIC_HDRS + include/mola_kernel/factors/FactorConstVelKinematics.h + include/mola_kernel/factors/SmartFactorStereoProjectionPose.h + include/mola_kernel/factors/FactorStereoProjectionPose.h + include/mola_kernel/factors/SmartFactorIMU.h + include/mola_kernel/factors/FactorRelativePose3.h + include/mola_kernel/factors/factors-common.h + include/mola_kernel/factors/FactorBase.h + include/mola_kernel/LazyLoadResource.h + include/mola_kernel/pretty_print_exception.h + include/mola_kernel/WorldModel.h + include/mola_kernel/variant_helper.h + include/mola_kernel/FastAllocator.h + include/mola_kernel/Entity.h + include/mola_kernel/interfaces/BackEndBase.h + include/mola_kernel/interfaces/FrontEndBase.h + include/mola_kernel/interfaces/VizInterface.h + include/mola_kernel/interfaces/ExecutableBase.h + include/mola_kernel/interfaces/Relocalization.h + include/mola_kernel/interfaces/MapServer.h + include/mola_kernel/interfaces/NavStateFilter.h + include/mola_kernel/interfaces/FilterBase.h + include/mola_kernel/interfaces/RawDataSourceBase.h + include/mola_kernel/interfaces/RawDataConsumer.h + include/mola_kernel/interfaces/OfflineDatasetSource.h + include/mola_kernel/interfaces/LocalizationSourceBase.h + include/mola_kernel/interfaces/MapSourceBase.h + include/mola_kernel/interfaces/Dataset_UI.h + include/mola_kernel/Factor.h + include/mola_kernel/entities/KeyFrameBase.h + include/mola_kernel/entities/EntityBase.h + include/mola_kernel/entities/RelPose3KF.h + include/mola_kernel/entities/RelPose3.h + include/mola_kernel/entities/EntityRelativeBase.h + include/mola_kernel/entities/RelDynPose3KF.h + include/mola_kernel/entities/LandmarkPoint3.h + include/mola_kernel/entities/RefPose3.h + include/mola_kernel/entities/entities-common.h + include/mola_kernel/id.h + include/mola_kernel/Yaml.h +) mola_add_library( TARGET ${PROJECT_NAME} @@ -41,3 +110,22 @@ mola_add_library( if("$ENV{VERBOSE}") message(STATUS "mola_kernel: Using MRPT version: ${MRPT_VERSION}") endif() + +#---- +# Extract version from package.xml +# Example line:" 0.3.2" +file(READ package.xml contentPackageXML) +string(REGEX MATCH "([0-9\.]*)" _ ${contentPackageXML}) +set(MOLA_VERSION ${CMAKE_MATCH_1}) +message(STATUS "MOLA version: ${MOLA_VERSION} (detected in package.xml)") +string(REGEX MATCH "^([0-9]+)\\.([0-9]+)\\.([0-9]+)" _ ${MOLA_VERSION}) +set(MOLA_MAJOR_VERSION ${CMAKE_MATCH_1}) +set(MOLA_MINOR_VERSION ${CMAKE_MATCH_2}) +set(MOLA_PATCH_VERSION ${CMAKE_MATCH_3}) +#---- + +message(STATUS "MOLA detected version (from package.xml): ${MOLA_VERSION}") + +target_compile_definitions(${PROJECT_NAME} PUBLIC MOLA_MAJOR_VERSION=${MOLA_MAJOR_VERSION}) +target_compile_definitions(${PROJECT_NAME} PUBLIC MOLA_MINOR_VERSION=${MOLA_MINOR_VERSION}) +target_compile_definitions(${PROJECT_NAME} PUBLIC MOLA_PATCH_VERSION=${MOLA_PATCH_VERSION}) diff --git a/mola_kernel/include/mola_kernel/interfaces/MapServer.h b/mola_kernel/include/mola_kernel/interfaces/MapServer.h new file mode 100644 index 0000000000..f37049bf6e --- /dev/null +++ b/mola_kernel/include/mola_kernel/interfaces/MapServer.h @@ -0,0 +1,65 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ +/** + * @file MapServer.h + * @brief Map saving/loading services offered by MOLA modules + * @author Jose Luis Blanco Claraco + * @date Aug 18, 2024 + */ +#pragma once + +#include +#include + +namespace mola +{ +/** Virtual interface for map server services offered by MOLA modules + * \ingroup mola_kernel_grp */ +class MapServer +{ + public: + MapServer(); + virtual ~MapServer(); + + /** @name Virtual interface of MapServer + *{ */ + + struct ReturnStatus + { + ReturnStatus() = default; + + bool success = false; + std::string error_message; + }; + + /** Loads a map from file(s) and sets it as active current map. + * Different implementations may use one or more files to store map as + * files. + * + * \param[in] path File name(s) prefix for the map to load. Do not add file + * extension. + */ + virtual ReturnStatus map_load([[maybe_unused]] const std::string& path) + { + return {}; + } + + /** Saves a map from file(s) and sets it as active current map. + * Different implementations may use one or more files to store map as + * files. + * + * \param[in] path File name(s) prefix for the map to save. Do not add file + * extension. + */ + virtual ReturnStatus map_save([[maybe_unused]] const std::string& path) + { + return {}; + } + + /** @} */ +}; + +} // namespace mola diff --git a/mola_kernel/include/mola_kernel/version.h b/mola_kernel/include/mola_kernel/version.h new file mode 100644 index 0000000000..a9d58a33eb --- /dev/null +++ b/mola_kernel/include/mola_kernel/version.h @@ -0,0 +1,19 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ +/** + * @file version.h + * @brief Provides macros for checking MOLA version + * @author Jose Luis Blanco Claraco + * @date Aug 18, 2024 + */ +#pragma once + +/// To be used in #if() checks for >= minimum MOLA versions +#define MOLA_VERSION_CHECK(major, minor, patch) \ + ((MOLA_MAJOR_VERSION > (major)) || \ + (MOLA_MAJOR_VERSION == (major) && MOLA_MINOR_VERSION > (minor)) || \ + (MOLA_MAJOR_VERSION == (major) && MOLA_MINOR_VERSION == (minor) && \ + MOLA_PATCH_VERSION >= (patch))) diff --git a/mola_kernel/package.xml b/mola_kernel/package.xml index 276e822cb5..0393c3a7a2 100644 --- a/mola_kernel/package.xml +++ b/mola_kernel/package.xml @@ -5,15 +5,14 @@ --> mola_kernel - 1.0.8 + 1.1.0 Fundamental C++ virtual interfaces and data types for the rest of MOLA modules - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_kernel - GPLv3 mola_common mola_yaml diff --git a/mola_kernel/src/Entity.cpp b/mola_kernel/src/Entity.cpp index 4e13a75402..33ed821293 100644 --- a/mola_kernel/src/Entity.cpp +++ b/mola_kernel/src/Entity.cpp @@ -21,7 +21,8 @@ EntityBase& mola::entity_get_base(Entity& e) std::visit( overloaded{ [&ret](EntityBase& b) { ret = &b; }, - [&ret](EntityOther& o) { + [&ret](EntityOther& o) + { ASSERT_(o); ret = o.get(); }, @@ -39,7 +40,8 @@ const EntityBase& mola::entity_get_base(const Entity& e) std::visit( overloaded{ [&ret](const EntityBase& b) { ret = &b; }, - [&ret](const EntityOther& o) { + [&ret](const EntityOther& o) + { ASSERT_(o); ret = o.get(); }, @@ -56,7 +58,8 @@ void mola::entity_update_pose(mola::Entity& e, const mrpt::math::TPose3D& p) std::visit( overloaded{ - [&](RefPose3&) { + [&](RefPose3&) + { ASSERTMSG_( p == mrpt::math::TPose3D::Identity(), "RefPose3 cannot be assigned a pose != Identity()"); @@ -64,7 +67,8 @@ void mola::entity_update_pose(mola::Entity& e, const mrpt::math::TPose3D& p) [&](RelDynPose3KF& ee) { ee.relpose_value = p; }, [&](RelPose3& ee) { ee.relpose_value = p; }, [&](RelPose3KF& ee) { ee.relpose_value = p; }, - []([[maybe_unused]] auto ee) { + []([[maybe_unused]] auto ee) + { throw std::runtime_error( mrpt::format("[updateEntityPose] Unknown Entity type!")); }, @@ -86,14 +90,16 @@ void mola::entity_update_vel(mola::Entity& e, const std::array& v) "RefPose3 cannot be assigned a velocity!=0"); #endif }, - [&](RelDynPose3KF& ee) { + [&](RelDynPose3KF& ee) + { ee.twist_value.vx = v[0]; ee.twist_value.vy = v[1]; ee.twist_value.vz = v[2]; }, [&](RelPose3&) {}, [&](RelPose3KF&) {}, - []([[maybe_unused]] auto ee) { + []([[maybe_unused]] auto ee) + { throw std::runtime_error( mrpt::format("[updateEntityPose] Unknown Entity type!")); }, @@ -113,7 +119,8 @@ mrpt::math::TTwist3D mola::entity_get_twist(const mola::Entity& e) [&](const RelDynPose3KF& ee) { ret = ee.twist_value; }, [&](const RelPose3&) {}, [&](const RelPose3KF&) {}, - []([[maybe_unused]] auto ee) { + []([[maybe_unused]] auto ee) + { throw std::runtime_error( mrpt::format("[updateEntityPose] Unknown Entity type!")); }, @@ -134,7 +141,8 @@ mrpt::math::TPose3D mola::entity_get_pose(const mola::Entity& e) [&](const RelDynPose3KF& ee) { ret = ee.relpose_value; }, [&](const RelPose3& ee) { ret = ee.relpose_value; }, [&](const RelPose3KF& ee) { ret = ee.relpose_value; }, - []([[maybe_unused]] auto ee) { + []([[maybe_unused]] auto ee) + { throw std::runtime_error( mrpt::format("[getEntityPose] Unknown Entity type!")); }, @@ -154,7 +162,8 @@ mrpt::Clock::time_point mola::entity_get_timestamp(const mola::Entity& e) overloaded{ [&](const EntityBase& ee) { ret = ee.timestamp_; }, [&](const EntityOther& ee) { ret = ee->timestamp_; }, - [](std::monostate) { + [](std::monostate) + { throw std::runtime_error( mrpt::format("[getEntityTimeStamp] Unknown Entity type!")); }, diff --git a/mola_kernel/src/Factor.cpp b/mola_kernel/src/Factor.cpp index bc4528dddf..87bd659062 100644 --- a/mola_kernel/src/Factor.cpp +++ b/mola_kernel/src/Factor.cpp @@ -21,7 +21,8 @@ FactorBase& mola::factor_get_base(Factor& f) std::visit( overloaded{ [&ret](FactorBase& b) { ret = &b; }, - [&ret](FactorOther& o) { + [&ret](FactorOther& o) + { ASSERT_(o); ret = o.get(); }, @@ -39,7 +40,8 @@ const FactorBase& mola::factor_get_base(const Factor& f) std::visit( overloaded{ [&ret](const FactorBase& b) { ret = &b; }, - [&ret](const FactorOther& o) { + [&ret](const FactorOther& o) + { ASSERT_(o); ret = o.get(); }, diff --git a/mola_kernel/src/factors/FactorConstVelKinematics.cpp b/mola_kernel/src/factors/FactorConstVelKinematics.cpp index c116218348..e53667c256 100644 --- a/mola_kernel/src/factors/FactorConstVelKinematics.cpp +++ b/mola_kernel/src/factors/FactorConstVelKinematics.cpp @@ -34,7 +34,7 @@ mola::id_t FactorConstVelKinematics::edge_indices(const std::size_t i) const // Implementation of the CSerializable virtual interface: uint8_t FactorConstVelKinematics::serializeGetVersion() const { return 0; } void FactorConstVelKinematics::serializeTo( - mrpt::serialization::CArchive& out) const + mrpt::serialization::CArchive& out) const { baseSerializeTo(out); out << from_kf_ << to_kf_ << deltaTime_; diff --git a/mola_kernel/src/factors/FactorStereoProjectionPose.cpp b/mola_kernel/src/factors/FactorStereoProjectionPose.cpp index 36ed25cad4..c2535cee00 100644 --- a/mola_kernel/src/factors/FactorStereoProjectionPose.cpp +++ b/mola_kernel/src/factors/FactorStereoProjectionPose.cpp @@ -29,7 +29,7 @@ mola::id_t FactorStereoProjectionPose::edge_indices(const std::size_t i) const // Implementation of the CSerializable virtual interface: uint8_t FactorStereoProjectionPose::serializeGetVersion() const { return 0; } void FactorStereoProjectionPose::serializeTo( - mrpt::serialization::CArchive& out) const + mrpt::serialization::CArchive& out) const { baseSerializeTo(out); out << sigma_xleft_ << sigma_xright_ << sigma_y_ << observation_.x_left diff --git a/mola_kernel/src/interfaces/FilterBase.cpp b/mola_kernel/src/interfaces/FilterBase.cpp index 5456ca7cf0..8d3f0615ef 100644 --- a/mola_kernel/src/interfaces/FilterBase.cpp +++ b/mola_kernel/src/interfaces/FilterBase.cpp @@ -43,7 +43,8 @@ void FilterBase::spinOnce() void FilterBase::onNewObservation(const CObservation::Ptr& o) { const auto obsFut = thread_pool_.enqueue( - [this](const CObservation::Ptr& in) { + [this](const CObservation::Ptr& in) + { try { // Process the observation: diff --git a/mola_kernel/src/interfaces/MapServer.cpp b/mola_kernel/src/interfaces/MapServer.cpp new file mode 100644 index 0000000000..1801a68a2d --- /dev/null +++ b/mola_kernel/src/interfaces/MapServer.cpp @@ -0,0 +1,19 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ +/** + * @file MapServer.cpp + * @brief Map saving/loading services offered by MOLA modules + * @author Jose Luis Blanco Claraco + * @date Aug 18, 2024 + */ + +#include + +namespace mola +{ +MapServer::MapServer() = default; +MapServer::~MapServer() = default; +} // namespace mola diff --git a/mola_kernel/src/interfaces/RawDataSourceBase.cpp b/mola_kernel/src/interfaces/RawDataSourceBase.cpp index 98c1ee1ba3..7c61826b91 100644 --- a/mola_kernel/src/interfaces/RawDataSourceBase.cpp +++ b/mola_kernel/src/interfaces/RawDataSourceBase.cpp @@ -143,7 +143,8 @@ void RawDataSourceBase::sendObservationsToFrontEnds( if (export_to_rawlog_out_.is_open()) { auto fut = worker_pool_export_rawlog_.enqueue( - [this](mrpt::obs::CObservation::Ptr o) { + [this](mrpt::obs::CObservation::Ptr o) + { if (!o) return; auto a = mrpt::serialization::archiveFrom( this->export_to_rawlog_out_); @@ -159,7 +160,8 @@ void RawDataSourceBase::sendObservationsToFrontEnds( // Create and enque the GUI update function, as a lambda: RawDataSourceBase::SensorViewerImpl* sv = &(*it_sen_gui->second); - auto func = [this, sv, obs]() { + auto func = [this, sv, obs]() + { try { ProfilerEntry pe(profiler_, "send to viz lambda"); diff --git a/mola_kernel/src/pretty_print_exception.cpp b/mola_kernel/src/pretty_print_exception.cpp index 9526bbca83..2839ce0e6e 100644 --- a/mola_kernel/src/pretty_print_exception.cpp +++ b/mola_kernel/src/pretty_print_exception.cpp @@ -26,7 +26,8 @@ void mola::pretty_print_exception( auto& o = use_std_cerr ? std::cerr : std::cout; - const auto setFormatRed = [=](const ConsoleTextStyle style) { + const auto setFormatRed = [=](const ConsoleTextStyle style) + { #if MRPT_VERSION >= 0x233 consoleColorAndStyle( ConsoleForegroundColor::RED, ConsoleBackgroundColor::DEFAULT, style, @@ -36,7 +37,8 @@ void mola::pretty_print_exception( #endif }; - const auto resetFormat = [=]() { + const auto resetFormat = [=]() + { #if MRPT_VERSION >= 0x233 consoleColorAndStyle( ConsoleForegroundColor::DEFAULT, ConsoleBackgroundColor::DEFAULT, diff --git a/mola_launcher/CHANGELOG.rst b/mola_launcher/CHANGELOG.rst index a5952bae5d..eff57bd945 100644 --- a/mola_launcher/CHANGELOG.rst +++ b/mola_launcher/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_launcher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_launcher/package.xml b/mola_launcher/package.xml index 255945ea81..62e3244836 100644 --- a/mola_launcher/package.xml +++ b/mola_launcher/package.xml @@ -5,15 +5,14 @@ --> mola_launcher - 1.0.8 + 1.1.0 Launcher app for MOLA systems - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_launcher - GPLv3 mola_kernel mrpt2 diff --git a/mola_launcher/src/MolaDLL_Loader.cpp b/mola_launcher/src/MolaDLL_Loader.cpp index 6660e0bb99..beaf4751c4 100644 --- a/mola_launcher/src/MolaDLL_Loader.cpp +++ b/mola_launcher/src/MolaDLL_Loader.cpp @@ -11,8 +11,10 @@ */ #include "MolaDLL_Loader.h" + #include #include + #include #if defined(__unix__) diff --git a/mola_launcher/src/MolaDLL_Loader.h b/mola_launcher/src/MolaDLL_Loader.h index 26c64893a1..fe5b14c9df 100644 --- a/mola_launcher/src/MolaDLL_Loader.h +++ b/mola_launcher/src/MolaDLL_Loader.h @@ -13,6 +13,7 @@ #pragma once #include + #include #include diff --git a/mola_metric_maps/CHANGELOG.rst b/mola_metric_maps/CHANGELOG.rst index 5f447770e6..866fc13227 100644 --- a/mola_metric_maps/CHANGELOG.rst +++ b/mola_metric_maps/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_metric_maps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * Update robin-map to latest version (Fix cmake < 3.5 compatibility warning) diff --git a/mola_metric_maps/apps/mola-myapp.cpp b/mola_metric_maps/apps/mola-myapp.cpp index b4499ba31d..55cf6cc535 100644 --- a/mola_metric_maps/apps/mola-myapp.cpp +++ b/mola_metric_maps/apps/mola-myapp.cpp @@ -11,23 +11,18 @@ * @date XXX */ -#include #include +#include + #include // Declare supported cli switches =========== static TCLAP::CmdLine cmd("mola-myapp"); static TCLAP::ValueArg argParam1( - "p", "param", - "Explanation", - false, "", "", cmd); + "p", "param", "Explanation", false, "", "", cmd); -static void do_my_app() -{ - using namespace std::string_literals; - -} +static void do_my_app() { using namespace std::string_literals; } int main(int argc, char** argv) { diff --git a/mola_metric_maps/package.xml b/mola_metric_maps/package.xml index c73d9da8e5..6507ad1bd5 100644 --- a/mola_metric_maps/package.xml +++ b/mola_metric_maps/package.xml @@ -5,15 +5,14 @@ --> mola_metric_maps - 1.0.8 + 1.1.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 Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_metric_maps - GPLv3 mola_common mrpt2 diff --git a/mola_metric_maps/src/SparseTreesPointCloud.cpp b/mola_metric_maps/src/SparseTreesPointCloud.cpp index f5c6596148..a387e642a5 100644 --- a/mola_metric_maps/src/SparseTreesPointCloud.cpp +++ b/mola_metric_maps/src/SparseTreesPointCloud.cpp @@ -110,7 +110,7 @@ IMPLEMENTS_SERIALIZABLE(SparseTreesPointCloud, CMetricMap, mola) uint8_t SparseTreesPointCloud::serializeGetVersion() const { return 0; } void SparseTreesPointCloud::serializeTo( - mrpt::serialization::CArchive& out) const + mrpt::serialization::CArchive& out) const { // params: out << GLOBAL_ID_SUBVOXEL_BITCOUNT << grid_size_; @@ -205,9 +205,8 @@ void SparseTreesPointCloud::getVisualizationInto( // Single color: auto obj = mrpt::opengl::CPointCloud::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); }; this->visitAllPoints(lambdaVisitPoints); obj->setColor(renderOptions.color); @@ -234,11 +233,12 @@ void SparseTreesPointCloud::getVisualizationInto( mrpt::math::CHistogram(bb.min.z, bb.max.z, nBins)}; const auto lambdaVisitPoints = - [&obj, &hists](const mrpt::math::TPoint3Df& pt) { - // x y z R G B [A] - obj->insertPoint({pt.x, pt.y, pt.z, 0, 0, 0}); - for (int i = 0; i < 3; i++) hists[i].add(pt[i]); - }; + [&obj, &hists](const mrpt::math::TPoint3Df& pt) + { + // x y z R G B [A] + obj->insertPoint({pt.x, pt.y, pt.z, 0, 0, 0}); + for (int i = 0; i < 3; i++) hists[i].add(pt[i]); + }; this->visitAllPoints(lambdaVisitPoints); @@ -266,16 +266,17 @@ void SparseTreesPointCloud::getVisualizationInto( if (renderOptions.show_inner_grid_boxes) { auto lambdaForEachGrid = - [this, &outObj](const outer_index3d_t& idxs, const GridData&) { - const mrpt::math::TPoint3Df gridCenter = outerIdxToCoord(idxs); + [this, &outObj](const outer_index3d_t& idxs, const GridData&) + { + const mrpt::math::TPoint3Df gridCenter = outerIdxToCoord(idxs); - auto glBox = mrpt::opengl::CBox::Create(); - glBox->setWireframe(true); - const auto org = gridCenter.cast(); - glBox->setBoxCorners(org, org + gridVector_); + auto glBox = mrpt::opengl::CBox::Create(); + glBox->setWireframe(true); + const auto org = gridCenter.cast(); + glBox->setBoxCorners(org, org + gridVector_); - outObj.insert(glBox); - }; + outObj.insert(glBox); + }; this->visitAllGrids(lambdaForEachGrid); } @@ -553,9 +554,8 @@ bool SparseTreesPointCloud::saveToTextFile(const std::string& file) const FILE* f = mrpt::system::os::fopen(file.c_str(), "wt"); if (!f) return false; - const auto lambdaVisitPoints = [f](const mrpt::math::TPoint3Df& pt) { - mrpt::system::os::fprintf(f, "%f %f %f\n", pt.x, pt.y, pt.z); - }; + const auto lambdaVisitPoints = [f](const mrpt::math::TPoint3Df& pt) + { mrpt::system::os::fprintf(f, "%f %f %f\n", pt.x, pt.y, pt.z); }; this->visitAllPoints(lambdaVisitPoints); @@ -672,7 +672,8 @@ void SparseTreesPointCloud::nn_radius_search( candidates[distSqr] = {pt, id}; }; - auto lambdaCheckCell = [&](const outer_index3d_t& p) { + auto lambdaCheckCell = [&](const outer_index3d_t& p) + { auto* g = gridByOuterIdxs(p, false); if (!g) return; @@ -727,7 +728,8 @@ mrpt::math::TBoundingBoxf SparseTreesPointCloud::boundingBox() const cached_.boundingBox_ = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); - auto lambdaForEachPt = [this](const mrpt::math::TPoint3Df& pt) { + auto lambdaForEachPt = [this](const mrpt::math::TPoint3Df& pt) + { cached_.boundingBox_->updateWithPoint(pt); cached_.boundingBox_->updateWithPoint(pt); }; @@ -970,7 +972,8 @@ void SparseTreesPointCloud::eraseGridsFartherThan( std::set> gridsToRemove; - auto lmbPerGrid = [&](const outer_index3d_t& idx, const GridData&) { + auto lmbPerGrid = [&](const outer_index3d_t& idx, const GridData&) + { if (idx.cx >= curIdxs0.cx && idx.cy >= curIdxs0.cy && idx.cz >= curIdxs0.cz && idx.cx <= curIdxs1.cx && idx.cy <= curIdxs1.cy && idx.cz <= curIdxs1.cz) diff --git a/mola_metric_maps/src/SparseVoxelPointCloud.cpp b/mola_metric_maps/src/SparseVoxelPointCloud.cpp index a98a0a23b0..b7614b9a71 100644 --- a/mola_metric_maps/src/SparseVoxelPointCloud.cpp +++ b/mola_metric_maps/src/SparseVoxelPointCloud.cpp @@ -110,7 +110,7 @@ IMPLEMENTS_SERIALIZABLE(SparseVoxelPointCloud, CMetricMap, mola) uint8_t SparseVoxelPointCloud::serializeGetVersion() const { return 0; } void SparseVoxelPointCloud::serializeTo( - mrpt::serialization::CArchive& out) const + mrpt::serialization::CArchive& out) const { // params: out << INNER_GRID_BIT_COUNT << voxel_size_; @@ -251,19 +251,18 @@ void SparseVoxelPointCloud::getVisualizationInto( const auto lambdaVisitVoxels = [&obj]( const outer_index3d_t&, const inner_plain_index_t, - const VoxelData& v, const InnerGrid& parentGrid) { - // Insert the mean/average point: - if (!v.points(parentGrid).empty()) - obj->insertPoint(v.mean()); - }; + const VoxelData& v, const InnerGrid& parentGrid) + { + // Insert the mean/average point: + if (!v.points(parentGrid).empty()) obj->insertPoint(v.mean()); + }; this->visitAllVoxels(lambdaVisitVoxels); } else { const auto lambdaVisitPoints = - [&obj](const mrpt::math::TPoint3Df& pt) { - obj->insertPoint(pt); - }; + [&obj](const mrpt::math::TPoint3Df& pt) + { obj->insertPoint(pt); }; this->visitAllPoints(lambdaVisitPoints); } @@ -295,22 +294,24 @@ void SparseVoxelPointCloud::getVisualizationInto( const auto lambdaVisitVoxels = [&obj, &hists]( const outer_index3d_t&, const inner_plain_index_t, - const VoxelData& v, const InnerGrid& parentGrid) { - if (v.points(parentGrid).empty()) return; - const auto& m = v.mean(); - obj->insertPoint({m.x, m.y, m.z, 0, 0, 0}); - for (int i = 0; i < 3; i++) hists[i].add(m[i]); - }; + const VoxelData& v, const InnerGrid& parentGrid) + { + if (v.points(parentGrid).empty()) return; + const auto& m = v.mean(); + obj->insertPoint({m.x, m.y, m.z, 0, 0, 0}); + for (int i = 0; i < 3; i++) hists[i].add(m[i]); + }; this->visitAllVoxels(lambdaVisitVoxels); } else { const auto lambdaVisitPoints = - [&obj, &hists](const mrpt::math::TPoint3Df& pt) { - // x y z R G B [A] - obj->insertPoint({pt.x, pt.y, pt.z, 0, 0, 0}); - for (int i = 0; i < 3; i++) hists[i].add(pt[i]); - }; + [&obj, &hists](const mrpt::math::TPoint3Df& pt) + { + // x y z R G B [A] + obj->insertPoint({pt.x, pt.y, pt.z, 0, 0, 0}); + for (int i = 0; i < 3; i++) hists[i].add(pt[i]); + }; this->visitAllPoints(lambdaVisitPoints); } @@ -339,18 +340,18 @@ void SparseVoxelPointCloud::getVisualizationInto( if (renderOptions.show_inner_grid_boxes) { auto lambdaForEachGrid = - [this, &outObj](const outer_index3d_t& idxs, const InnerGrid&) { - const mrpt::math::TPoint3Df voxelCenter = - globalIdxToCoord(idxs); + [this, &outObj](const outer_index3d_t& idxs, const InnerGrid&) + { + const mrpt::math::TPoint3Df voxelCenter = globalIdxToCoord(idxs); - auto glBox = mrpt::opengl::CBox::Create(); - glBox->setWireframe(true); - glBox->setBoxCorners( - (voxelCenter - halfVoxel_).cast(), - (voxelCenter + gridSizeMinusHalf_).cast()); + auto glBox = mrpt::opengl::CBox::Create(); + glBox->setWireframe(true); + glBox->setBoxCorners( + (voxelCenter - halfVoxel_).cast(), + (voxelCenter + gridSizeMinusHalf_).cast()); - outObj.insert(glBox); - }; + outObj.insert(glBox); + }; this->visitAllGrids(lambdaForEachGrid); } @@ -625,9 +626,8 @@ bool SparseVoxelPointCloud::saveToTextFile(const std::string& file) const FILE* f = mrpt::system::os::fopen(file.c_str(), "wt"); if (!f) return false; - const auto lambdaVisitPoints = [f](const mrpt::math::TPoint3Df& pt) { - mrpt::system::os::fprintf(f, "%f %f %f\n", pt.x, pt.y, pt.z); - }; + const auto lambdaVisitPoints = [f](const mrpt::math::TPoint3Df& pt) + { mrpt::system::os::fprintf(f, "%f %f %f\n", pt.x, pt.y, pt.z); }; this->visitAllPoints(lambdaVisitPoints); @@ -741,8 +741,9 @@ void SparseVoxelPointCloud::nn_radius_search( float /*distSqr*/, std::pair> candidates; - auto lmbAddCandidate = [&](const float distSqr, - const mrpt::math::TPoint3Df& pt, uint64_t id) { + auto lmbAddCandidate = + [&](const float distSqr, const mrpt::math::TPoint3Df& pt, uint64_t id) + { if (maxPoints != 0) { candidates[distSqr] = {pt, id}; } else { @@ -752,7 +753,8 @@ void SparseVoxelPointCloud::nn_radius_search( } }; - auto lambdaCheckCell = [&](const global_index3d_t& p) { + auto lambdaCheckCell = [&](const global_index3d_t& p) + { if (auto [v, grid] = voxelByGlobalIdxs(p, false); v && !v->points(*grid).empty()) { @@ -812,9 +814,9 @@ mrpt::math::TBoundingBoxf SparseVoxelPointCloud::boundingBox() const cached_.boundingBox_ = mrpt::math::TBoundingBoxf::PlusMinusInfinity(); - auto lambdaForEachGrid = [this]( - const outer_index3d_t& idxs, - const InnerGrid&) { + auto lambdaForEachGrid = + [this](const outer_index3d_t& idxs, const InnerGrid&) + { const mrpt::math::TPoint3Df voxelCenter = globalIdxToCoord(idxs); diff --git a/mola_metric_maps/tests/test-mola_metric_maps_hashedvoxels.cpp b/mola_metric_maps/tests/test-mola_metric_maps_hashedvoxels.cpp index 64b9341e60..61d953ef00 100644 --- a/mola_metric_maps/tests/test-mola_metric_maps_hashedvoxels.cpp +++ b/mola_metric_maps/tests/test-mola_metric_maps_hashedvoxels.cpp @@ -59,9 +59,8 @@ void test_voxelmap_insert_2d_scan() { size_t nPts = 0; - const auto lambdaVisitPoints = [&nPts](const mrpt::math::TPoint3Df&) { - nPts++; - }; + const auto lambdaVisitPoints = [&nPts](const mrpt::math::TPoint3Df&) + { nPts++; }; map.visitAllPoints(lambdaVisitPoints); @@ -70,8 +69,8 @@ void test_voxelmap_insert_2d_scan() // test NN search: mrpt::maps::CSimplePointsMap refPts; - map.visitAllPoints( - [&](const mrpt::math::TPoint3Df& pt) { refPts.insertPoint(pt); }); + map.visitAllPoints([&](const mrpt::math::TPoint3Df& pt) + { refPts.insertPoint(pt); }); mrpt::maps::CSimplePointsMap queryPts; queryPts.insertObservation( diff --git a/mola_metric_maps/tests/test-sparsevoxelpointcloud.cpp b/mola_metric_maps/tests/test-sparsevoxelpointcloud.cpp index 1a79e60c2c..96bca20382 100644 --- a/mola_metric_maps/tests/test-sparsevoxelpointcloud.cpp +++ b/mola_metric_maps/tests/test-sparsevoxelpointcloud.cpp @@ -65,9 +65,8 @@ void test_voxelmap_insert_2d_scan() { size_t nPts = 0; - const auto lambdaVisitPoints = [&nPts](const mrpt::math::TPoint3Df&) { - nPts++; - }; + const auto lambdaVisitPoints = [&nPts](const mrpt::math::TPoint3Df&) + { nPts++; }; map.visitAllPoints(lambdaVisitPoints); @@ -82,10 +81,11 @@ void test_voxelmap_insert_2d_scan() const mola::SparseVoxelPointCloud::outer_index3d_t&, const mola::SparseVoxelPointCloud::inner_plain_index_t, const mola::SparseVoxelPointCloud::VoxelData& v, - const mola::SparseVoxelPointCloud::InnerGrid& grid) { - // count them: - if (!v.points(grid).empty()) nVoxels++; - }; + const mola::SparseVoxelPointCloud::InnerGrid& grid) + { + // count them: + if (!v.points(grid).empty()) nVoxels++; + }; map.visitAllVoxels(lambdaVisitVoxels); ASSERT_EQUAL_(nVoxels, 96UL); diff --git a/mola_msgs/CHANGELOG.rst b/mola_msgs/CHANGELOG.rst index 6a4ccb0c7e..e3144877ed 100644 --- a/mola_msgs/CHANGELOG.rst +++ b/mola_msgs/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package mola_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#65 `_ from MOLAorg/add-more-srvs + Add more Services +* mola_msgs: add map save & load services +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * Add mola_msgs package with ROS service definitions diff --git a/mola_msgs/CMakeLists.txt b/mola_msgs/CMakeLists.txt index cfc30c41d4..f1eb5e16e3 100644 --- a/mola_msgs/CMakeLists.txt +++ b/mola_msgs/CMakeLists.txt @@ -18,6 +18,8 @@ find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/RelocalizeFromGNSS.srv" "srv/RelocalizeNearPose.srv" + "srv/MapLoad.srv" + "srv/MapSave.srv" DEPENDENCIES std_msgs geometry_msgs diff --git a/mola_msgs/package.xml b/mola_msgs/package.xml index b52b2ada6e..8b400a17f2 100644 --- a/mola_msgs/package.xml +++ b/mola_msgs/package.xml @@ -2,14 +2,14 @@ mola_msgs - 1.0.8 + 1.1.0 ROS message, services, and actions used in other MOLA packages. Jose Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_msgs - GPLv3 ament_cmake diff --git a/mola_msgs/srv/MapLoad.srv b/mola_msgs/srv/MapLoad.srv new file mode 100644 index 0000000000..724a8f6239 --- /dev/null +++ b/mola_msgs/srv/MapLoad.srv @@ -0,0 +1,12 @@ +# This service requests loading a map from a given map file(s). + +# Request. +# File relative or absolute path. Do not add any file extension, since maps +# may be loaded/saved to several files depending on implementation. +string map_path +--- +# Result. False if there was no MOLA map module accepting this request, or +# there was an error loading the map. +bool success +# In case of no success, here is a human-readable error reason. +string error_message diff --git a/mola_msgs/srv/MapSave.srv b/mola_msgs/srv/MapSave.srv new file mode 100644 index 0000000000..294919a0b5 --- /dev/null +++ b/mola_msgs/srv/MapSave.srv @@ -0,0 +1,12 @@ +# This service requests saving the current map to a given map file(s). + +# Request. +# File relative or absolute path. Do not add any file extension, since maps +# may be loaded/saved to several files depending on implementation. +string map_path +--- +# Result. False if there was no MOLA map module accepting this request, or +# there was an error saving the map. +bool success +# In case of no success, here is a human-readable error reason. +string error_message diff --git a/mola_navstate_fg/CHANGELOG.rst b/mola_navstate_fg/CHANGELOG.rst index aae2a3feda..e4e8937c59 100644 --- a/mola_navstate_fg/CHANGELOG.rst +++ b/mola_navstate_fg/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_navstate_fg ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update test-navstate-basic.cpp: less noisy test data for more predictable results +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_navstate_fg/package.xml b/mola_navstate_fg/package.xml index 98d76572d3..dd6b0d1f96 100644 --- a/mola_navstate_fg/package.xml +++ b/mola_navstate_fg/package.xml @@ -5,21 +5,20 @@ --> mola_navstate_fg - 1.0.8 + 1.1.0 SE(3) pose and twist path data fusion estimator - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_navstate_fuse - GPLv3 mola_common mola_kernel mola_imu_preintegration mrpt2 - + gtsam libboost-serialization-dev diff --git a/mola_navstate_fg/tests/test-navstate-basic.cpp b/mola_navstate_fg/tests/test-navstate-basic.cpp index 0454fbb9dc..395b663367 100644 --- a/mola_navstate_fg/tests/test-navstate-basic.cpp +++ b/mola_navstate_fg/tests/test-navstate-basic.cpp @@ -279,8 +279,8 @@ void test_noisy_straight() const double vx = 8.0; // m/s const double T = 0.100; // s - const double stdXYZ = 0.03; - const double stdYPR = mrpt::DEG2RAD(0.2); + const double stdXYZ = 0.01; + const double stdYPR = mrpt::DEG2RAD(0.1); for (size_t i = 0; i < nSteps; i++) { diff --git a/mola_navstate_fuse/CHANGELOG.rst b/mola_navstate_fuse/CHANGELOG.rst index 5ecfb2cf1c..f4528ae478 100644 --- a/mola_navstate_fuse/CHANGELOG.rst +++ b/mola_navstate_fuse/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package mola_navstate_fuse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_navstate_fuse/package.xml b/mola_navstate_fuse/package.xml index 5e66ad8ace..bebd4b5ab2 100644 --- a/mola_navstate_fuse/package.xml +++ b/mola_navstate_fuse/package.xml @@ -5,15 +5,14 @@ --> mola_navstate_fuse - 1.0.8 + 1.1.0 SE(3) pose and twist path data fusion estimator - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_navstate_fuse - GPLv3 mola_common mola_kernel diff --git a/mola_pose_list/CHANGELOG.rst b/mola_pose_list/CHANGELOG.rst index 82fc625f22..4b9b2316c3 100644 --- a/mola_pose_list/CHANGELOG.rst +++ b/mola_pose_list/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package mola_pose_list ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_pose_list/package.xml b/mola_pose_list/package.xml index 2043b5a49a..31cdff3047 100644 --- a/mola_pose_list/package.xml +++ b/mola_pose_list/package.xml @@ -5,15 +5,14 @@ --> mola_pose_list - 1.0.8 + 1.1.0 C++ library for searchable pose lists - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_pose_list - GPLv3 mola_common mrpt2 diff --git a/mola_relocalization/CHANGELOG.rst b/mola_relocalization/CHANGELOG.rst index 085b56f2c2..96cf3b21be 100644 --- a/mola_relocalization/CHANGELOG.rst +++ b/mola_relocalization/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package mola_relocalization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * BUGFIX: Add missing cmake dependency on mrpt-slam diff --git a/mola_relocalization/package.xml b/mola_relocalization/package.xml index da719589c3..1399bbe31b 100644 --- a/mola_relocalization/package.xml +++ b/mola_relocalization/package.xml @@ -5,15 +5,14 @@ --> mola_relocalization - 1.0.8 + 1.1.0 C++ library with algorithms for relocalization, global localization, or pose estimation given a large initial uncertainty - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_relocalization - GPLv3 mola_common mrpt2 diff --git a/mola_traj_tools/CHANGELOG.rst b/mola_traj_tools/CHANGELOG.rst index 277d2f0f74..b864ff1a13 100644 --- a/mola_traj_tools/CHANGELOG.rst +++ b/mola_traj_tools/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_traj_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_traj_tools/package.xml b/mola_traj_tools/package.xml index bb430407f5..398427d2be 100644 --- a/mola_traj_tools/package.xml +++ b/mola_traj_tools/package.xml @@ -5,15 +5,14 @@ --> mola_traj_tools - 1.0.8 + 1.1.0 CLI tools to manipulate trajectory files as a complement to the evo package - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + BSD https://github.com/MOLAorg/mola/tree/develop/mola_traj_tools - BSD mola_common mrpt2 diff --git a/mola_traj_tools/src/traj_ypr2tum.cpp b/mola_traj_tools/src/traj_ypr2tum.cpp index c66f441246..97ccda19e7 100644 --- a/mola_traj_tools/src/traj_ypr2tum.cpp +++ b/mola_traj_tools/src/traj_ypr2tum.cpp @@ -15,8 +15,7 @@ int main(int argc, char** argv) { if (argc != 3) { - std::cerr << "Usage: " << argv[0] << " INPUT.ypr OUTPUT.tum" - << std::endl; + std::cerr << "Usage: " << argv[0] << " INPUT.ypr OUTPUT.tum" << std::endl; return 1; } diff --git a/mola_viz/CHANGELOG.rst b/mola_viz/CHANGELOG.rst index 573ea57f0e..b515adb135 100644 --- a/mola_viz/CHANGELOG.rst +++ b/mola_viz/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_viz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_viz/package.xml b/mola_viz/package.xml index b707ac3466..4da3752109 100644 --- a/mola_viz/package.xml +++ b/mola_viz/package.xml @@ -5,15 +5,14 @@ --> mola_viz - 1.0.8 + 1.1.0 GUI for MOLA - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + GPLv3 https://github.com/MOLAorg/mola/tree/develop/mola_viz - GPLv3 mola_kernel mrpt2 diff --git a/mola_viz/src/mola_icon_64x64.h b/mola_viz/src/mola_icon_64x64.h index 272aad95df..2c42373ffb 100644 --- a/mola_viz/src/mola_icon_64x64.h +++ b/mola_viz/src/mola_icon_64x64.h @@ -1,16 +1,18 @@ /* GIMP header image file format (RGB) */ -static const unsigned int mola_icon_width = 64; +static const unsigned int mola_icon_width = 64; static const unsigned int mola_icon_height = 64; -/* Call this macro repeatedly. After each use, the pixel data can be extracted */ +/* Call this macro repeatedly. After each use, the pixel data can be extracted + */ -#define HEADER_PIXEL(data,pixel) {\ -pixel[0] = (((data[0] - 33) << 2) | ((data[1] - 33) >> 4)); \ -pixel[1] = ((((data[1] - 33) & 0xF) << 4) | ((data[2] - 33) >> 2)); \ -pixel[2] = ((((data[2] - 33) & 0x3) << 6) | ((data[3] - 33))); \ -data += 4; \ -} +#define HEADER_PIXEL(data, pixel) \ + { \ + pixel[0] = (((data[0] - 33) << 2) | ((data[1] - 33) >> 4)); \ + pixel[1] = ((((data[1] - 33) & 0xF) << 4) | ((data[2] - 33) >> 2)); \ + pixel[2] = ((((data[2] - 33) & 0x3) << 6) | ((data[3] - 33))); \ + data += 4; \ + } // clang-format off static const char *mola_icon_data = "````````````````````````````````````````````````````````````````" diff --git a/mola_yaml/CHANGELOG.rst b/mola_yaml/CHANGELOG.rst index c59e27f7a7..f30e83da28 100644 --- a/mola_yaml/CHANGELOG.rst +++ b/mola_yaml/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mola_yaml ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.0 (2024-08-18) +------------------ +* Update clang-format style; add reformat bash script +* Merge pull request `#62 `_ from MOLAorg/docs-fixes + Docs fixes +* Fix ament_xmllint warnings in package.xml +* Contributors: Jose Luis Blanco-Claraco + 1.0.8 (2024-07-29) ------------------ * ament_lint_cmake: clean warnings diff --git a/mola_yaml/package.xml b/mola_yaml/package.xml index 1fc6d91394..6a0efba881 100644 --- a/mola_yaml/package.xml +++ b/mola_yaml/package.xml @@ -5,15 +5,14 @@ --> mola_yaml - 1.0.8 + 1.1.0 YAML helper library common to MOLA modules - Jose-Luis Blanco-Claraco Jose-Luis Blanco-Claraco + BSD https://github.com/MOLAorg/mola/tree/develop/mola_yaml - BSD mola_common mrpt2 diff --git a/mola_yaml/src/yaml_helpers.cpp b/mola_yaml/src/yaml_helpers.cpp index 91275ed46a..9e8fed6254 100644 --- a/mola_yaml/src/yaml_helpers.cpp +++ b/mola_yaml/src/yaml_helpers.cpp @@ -116,10 +116,7 @@ static std::string parseEnvVars( // ${CURRENT_YAML_FILE_PATH} if (varname == "CURRENT_YAML_FILE_PATH") varvalue = opts.includesBasePath; - else if (!defaultValue.empty()) - { - varvalue = defaultValue; - } + else if (!defaultValue.empty()) { varvalue = defaultValue; } else { THROW_EXCEPTION_FMT( diff --git a/scripts/clang-formatter.sh b/scripts/clang-formatter.sh new file mode 100755 index 0000000000..2b8f074150 --- /dev/null +++ b/scripts/clang-formatter.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +find kitti*/ mola*/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs -I FIL bash -c "echo FIL && clang-format-14 -i FIL"