diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml new file mode 100644 index 00000000..51cdc493 --- /dev/null +++ b/.github/workflows/humble.yaml @@ -0,0 +1,48 @@ +name: humble + +on: + pull_request: + branches: + - humble + push: + branches: + - humble +jobs: + build-and-test: + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: humble + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + target-ros2-distro: humble + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml new file mode 100644 index 00000000..82bb57ad --- /dev/null +++ b/.github/workflows/humble_cron.yaml @@ -0,0 +1,45 @@ +name: humble + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: humble + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + target-ros2-distro: humble + ref: humble + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 33198952..365cdccf 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -11,11 +11,9 @@ on: - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - name: Repo checkout uses: actions/checkout@v4 @@ -23,16 +21,12 @@ jobs: ref: jazzy - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: jazzy - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml new file mode 100644 index 00000000..c7049a98 --- /dev/null +++ b/.github/workflows/jazzy_cron.yaml @@ -0,0 +1,43 @@ +name: jazzy + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: jazzy + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + target-ros2-distro: jazzy + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index f699529f..0a4f68ed 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -7,15 +7,11 @@ on: push: branches: - kilted - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - name: Repo checkout uses: actions/checkout@v4 @@ -23,16 +19,12 @@ jobs: ref: kilted - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: kilted - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: kilted - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml new file mode 100644 index 00000000..c4b6b82b --- /dev/null +++ b/.github/workflows/kilted_cron.yaml @@ -0,0 +1,43 @@ +name: kilted + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: kilted + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + target-ros2-distro: kilted + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 30cd778d..7efd701a 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -7,32 +7,25 @@ on: push: branches: - rolling - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - name: Repo checkout - uses: actions/checkout@v4 + uses: actions/checkout@v6-beta with: ref: rolling - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: rolling - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml new file mode 100644 index 00000000..ef59286d --- /dev/null +++ b/.github/workflows/rolling_cron.yaml @@ -0,0 +1,45 @@ +name: rolling + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble + steps: + - name: Repo checkout + uses: actions/checkout@v6-beta + with: + ref: rolling + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + target-ros2-distro: rolling + ref: rolling + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: false + colcon-defaults: | + { + "build": { + "packages-up-to": true, + "mixin": ["coverage-gcc"] + }, + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..46e553ed --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +# VS Code stuff +/.vscode/** +**/__pycache__/ + +# ROS 2 build files +build/ +install/ +log/ diff --git a/README.md b/README.md index df009e83..583ff9a6 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,12 @@ [![rolling](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/rolling.yaml/badge.svg?branch=rolling)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/rolling.yaml) [![kilted](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/kilted.yaml/badge.svg?branch=kilted)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/kilted.yaml) [![jazzy](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/jazzy.yaml/badge.svg?branch=jazzy)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/jazzy.yaml) +[![humble](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/humble.yaml/badge.svg?branch=humble)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/humble.yaml) + +📋 Roadmap Project: [RoadMap](https://github.com/EasyNavigation/EasyNavigation/blob/rolling/ROADMAP.md) ## Description + **EasyNav Plugins** provides the official collection of plugins for the [Easy Navigation (EasyNav)](https://github.com/EasyNavigation) framework. These plugins extend the navigation core with planners, controllers, map managers, and localizers compatible with ROS 2. @@ -16,6 +20,7 @@ Each plugin resides in its own ROS 2 package and is registered via `pluginlib`, ## Repository Structure ### 🧭 Planners + Path planning plugins implementing A*, costmap, or NavMap–based methods. | Package | Description | Link | @@ -27,6 +32,7 @@ Path planning plugins implementing A*, costmap, or NavMap–based methods. --- ### ⚙️ Controllers + Motion controllers for trajectory tracking and reactive behaviors. | Package | Description | Link | @@ -35,10 +41,12 @@ Motion controllers for trajectory tracking and reactive behaviors. | `easynav_mppi_controller` | Model Predictive Path Integral (MPPI) controller. | [README](./controllers/easynav_mppi_controller/README.md) | | `easynav_simple_controller` | Simple proportional controller for testing. | [README](./controllers/easynav_simple_controller/README.md) | | `easynav_serest_controller` | SeReST (Safe Reactive Steering) controller. | [README](./controllers/easynav_serest_controller/README.md) | +| `easynav_mpc_controller` | Model Predictive Controller (MPC). | [README](./controllers/easynav_mpc_controller/README.md) | --- ### 🗺️ Maps Managers + Map management plugins that provide, update, and store different environment representations. | Package | Description | Link | @@ -52,6 +60,7 @@ Map management plugins that provide, update, and store different environment rep --- ### 📍 Localizers + Localization plugins based on different map types and sensors. | Package | Description | Link | @@ -60,9 +69,10 @@ Localization plugins based on different map types and sensors. | `easynav_simple_localizer` | Basic localizer for SimpleMap–based setups. | [README](./localizers/easynav_simple_localizer/README.md) | | `easynav_navmap_localizer` | AMCL-like localizer operating on NavMap meshes. | [README](./localizers/easynav_navmap_localizer/README.md) | | `easynav_costmap_localizer` | AMCL-like localizer using Costmap2D. | [README](./localizers/easynav_costmap_localizer/README.md) | +| `easynav_fusion_localizer` | Multi-sensor fusion localizer (e.g., GPS + odometry + map). | [README](./localizers/easynav_fusion_localizer/README.md) | --- ## License -All packages in this repository are released under **GPL-3.0-only** unless stated otherwise in the individual package. +All packages in this repository are released under **GPL-3.0-only** unless stated otherwise in the individual package. diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp index d141f071..45394e9c 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp @@ -38,14 +38,11 @@ #ifndef EASYNAV_COSTMAP_COMMON__COSTMAP_2D_HPP_ #define EASYNAV_COSTMAP_COMMON__COSTMAP_2D_HPP_ -#include -#include -#include +#include #include #include -#include +#include #include -#include #include #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp index e7f4b640..744f724a 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/geometry_utils.hpp @@ -16,7 +16,6 @@ #define EASYNAV_COSTMAP_COMMON__GEOMETRY_UTILS_HPP_ #include -#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp index a563202e..9834ef89 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/occ_grid_values.hpp @@ -35,6 +35,7 @@ #ifndef EASYNAV_COSTMAP_COMMON__OCC_GRID_VALUES_HPP_ #define EASYNAV_COSTMAP_COMMON__OCC_GRID_VALUES_HPP_ +#include namespace easynav { diff --git a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp index aeb8b088..91fd5928 100644 --- a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp +++ b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp @@ -39,11 +39,7 @@ #include #include -#include #include -#include -#include -#include #include "easynav_costmap_common/cost_values.hpp" #include "easynav_costmap_common/occ_grid_values.hpp" diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index 70e969d9..955f9514 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -19,7 +19,6 @@ #include #include "easynav_costmap_common/costmap_2d.hpp" -#include "easynav_costmap_common/cost_values.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" using easynav::Costmap2D; diff --git a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp index 331a820c..c34ad138 100644 --- a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp +++ b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp @@ -24,11 +24,7 @@ #define EASYNAV_PLANNER__SIMPLEMAP_HPP_ #include -#include -#include #include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp b/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp index e469b9c1..e00da478 100644 --- a/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp +++ b/common/easynav_simple_common/src/easynav_simple_common/SimpleMap.cpp @@ -296,7 +296,8 @@ SimpleMap::downsample_factor(int factor) const } } - new_map->at(x, y) = (occupied_count > (factor * factor / 2)); + // Mark cell as occupied if any source cell in the block is occupied + new_map->at(x, y) = (occupied_count > 0); } } diff --git a/common/easynav_simple_common/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index d3f97cb6..7ad1b184 100644 --- a/common/easynav_simple_common/tests/simple_maps_tests.cpp +++ b/common/easynav_simple_common/tests/simple_maps_tests.cpp @@ -21,8 +21,6 @@ #include "easynav_simple_common/SimpleMap.hpp" -#include - /// \brief Fixture for SimpleMap tests (minimal) class SimpleMapTest : public ::testing::Test { @@ -174,7 +172,7 @@ TEST_F(SimpleMapTest, DownsampleIntegerFactor) EXPECT_TRUE(downsampled->at(0, 0)); EXPECT_FALSE(downsampled->at(1, 0)); EXPECT_FALSE(downsampled->at(0, 1)); - EXPECT_FALSE(downsampled->at(1, 1)); + EXPECT_TRUE(downsampled->at(1, 1)); } /// \brief Downsampling to new resolution @@ -197,9 +195,9 @@ TEST_F(SimpleMapTest, DownsampleToResolution) EXPECT_DOUBLE_EQ(downsampled->origin_x(), -1.0); EXPECT_DOUBLE_EQ(downsampled->origin_y(), -1.0); - EXPECT_FALSE(downsampled->at(0, 0)); - EXPECT_FALSE(downsampled->at(1, 1)); - EXPECT_FALSE(downsampled->at(2, 2)); + EXPECT_TRUE(downsampled->at(0, 0)); + EXPECT_TRUE(downsampled->at(1, 1)); + EXPECT_TRUE(downsampled->at(2, 2)); } /// \brief Downsample that results in cropped edges diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt new file mode 100644 index 00000000..ef47e161 --- /dev/null +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -0,0 +1,95 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_mpc_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_system REQUIRED) +find_package(pluginlib REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(PCL REQUIRED COMPONENTS common io) +find_package(NLopt QUIET) +if(NOT NLopt_FOUND) + find_path(NLOPT_INCLUDE_DIR nlopt.hpp) + find_library(NLOPT_LIBRARY nlopt) + if(NLOPT_INCLUDE_DIR AND NLOPT_LIBRARY) + set(NLopt_FOUND TRUE) + set(NLopt_INCLUDE_DIRS ${NLOPT_INCLUDE_DIR}) + set(NLopt_LIBRARIES ${NLOPT_LIBRARY}) + endif() +endif() +if(NOT NLopt_FOUND) + message(FATAL_ERROR "NLopt not found. Install libnlopt-dev or set NLopt_DIR/CMAKE_PREFIX_PATH.") +endif() + +add_library(${PROJECT_NAME} SHARED + src/easynav_mpc_controller/MPCController.cpp + src/easynav_mpc_controller/MPCOptimizer.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + easynav_system::easynav_system + tf2_ros::tf2_ros + pluginlib::pluginlib + Eigen3::Eigen + nlopt + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${PCL_LIBRARIES} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + # add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the planning plugins +pluginlib_export_plugin_description_file(easynav_core easynav_mpc_controller_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + tf2_ros + geometry_msgs + nav_msgs + eigen + nlopt +) + +ament_package() diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md new file mode 100644 index 00000000..a707baee --- /dev/null +++ b/controllers/easynav_mpc_controller/README.md @@ -0,0 +1,64 @@ +# easynav_mpc_controller + +[![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) + + +## Description +A Model Predictive Controller (MPC) implementation for Easy Navigation. + +## Authors and Maintainers +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Juan S. Cely G. + +## Supported ROS 2 Distributions +| Distribution | Status | +|---|---| +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | + +## Plugin (pluginlib) +- **Plugin Name:** `easynav_mpc_controller/MPCController` +- **Type:** `easynav::MPCController` +- **Base Class:** `easynav::ControllerMethodBase` +- **Library:** `easynav_mpc_controller` +- **Description:** A Model Predictive Controller (MPC) implementation for Easy Navigation. + +## Parameters +All parameters are declared under the plugin namespace, i.e., `//easynav_mpc_controller/MPCController/...`. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.horizon_steps` | `int` | `5` | Number of time steps in the prediction horizon. | +| `.dt` | `double` | `0.1` | Integration time step (seconds). | +| `.safety_radius` | `double` | `0.35` | Safety radius to check possible collisions. | +| `.max_linear_velocity` | `double` | `1.5` | Maximum linear velocity (m/s). | +| `.max_angular_velocity` | `double` | `1.5` | Maximum angular velocity (rad/s). | +| `.verbose` | `bool` | `false` | Show data on terminal about Optimization. | + + +## Interfaces (Topics and Services) + +### Subscriptions and Publications +| Direction | Topic | Type | Purpose | QoS | +|---|---|---|---|---| +| Publisher | `/mpc/path` | `nav_msgs/msg/path` | MPC trajectories generate by Predictive Component. | QoS depth=10 | +| Publisher | `/mpc/detection` | `sensor_msgs::msg::PointCloud2` | Detections used by collision checker. | QoS depth=10 | + + +### Services +This package does not create service servers or clients. + + +## NavState Keys +| Key | Type | Access | Notes | +|---|---|---|---| +| `path` | `nav_msgs::msg::Path` | **Read** | Target path to track. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose/state. | +| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Read** | Last commanded velocity (if provided in state). | +| `points` | `PointPerceptions` | **Read** | Perception point cloud(s) used for costs. | + + +## TF Frames +This controller does not explicitly publish or require TF frames in code. + +## License +GPL-3.0-only diff --git a/controllers/easynav_mpc_controller/easynav_mpc_controller_plugins.xml b/controllers/easynav_mpc_controller/easynav_mpc_controller_plugins.xml new file mode 100644 index 00000000..cebf350e --- /dev/null +++ b/controllers/easynav_mpc_controller/easynav_mpc_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + A MPC implementation for a Controller. + + + + diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp new file mode 100644 index 00000000..cee5a66d --- /dev/null +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -0,0 +1,96 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. + +// #pragma once +#ifndef EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ +#define EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ + +#include +#include +#include +#include + +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" + +#include +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include "easynav_core/ControllerMethodBase.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_common/types/PointPerception.hpp" + +#include "easynav_mpc_controller/MPCOptimizer.hpp" + +namespace easynav +{ + +/// \brief A MPC Controller. +class MPCController : public ControllerMethodBase +{ +public: + MPCController(); + + /// \brief Destructor. + ~MPCController() override; + + /// \brief Initializes parameters and MPC controller. + /// \throws std::runtime_error if initialization fails. + void on_initialize() override; + + /// \brief Updates the controller using the given NavState. + /// \param nav_state Current navigation state, including odometry and planned path. + void update_rt(NavState & nav_state) override; + + /// \brief Publishes the selected path by MPC controller + /// \param data MPCParameters object as data pointer + /// \param best_path Vector of velocities generated by MPC + /// \param path Path obtained from navstate + void publish_mpc_path( + void *data, const std::vector & best_path, + nav_msgs::msg::Path path); + + /// \brief Check if there is a possible collision + /// \param data MPCParameters object as data pointer + /// \param u Vector of velocities generated by MPC + void collision_checker(void *data, std::vector & u); + +protected: + int horizon_steps_{5}; ///< Prediction horizon for MPC. + double dt_{0.1}; ///< Time step for MPC. + double safety_radius_{0.35}; ///< Safety radius to avoid obstacles + double max_lin_vel_{1.5}; ///< Maximum linear velocity for MPC. + double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. + bool verbose_{false}; ///< Value to debug on terminal + double last_v_{0.0}; ///< Last value for linear velocity before than collision + double last_w_{0.0}; ///< Last value for angular velocity before than collision + bool collision_state_{false}; ///< Collision state flag + double collision_factor_{0.618033}; ///< Collision avoidance for recalculate velocities + + // Fallback goal tolerances if GoalManager does not publish them + double fallback_goal_pos_tol_{0.05}; ///< Default positional tolerance (meters). + double fallback_goal_yaw_tol_{0.05}; ///< Default angular tolerance (radians). + + std::unique_ptr optimizer_; ///< MPC optimizer + + rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path. + rclcpp::Publisher::SharedPtr detection_pub_; ///< Publisher for MPC obstacles. + +private: + geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. + +}; + +} // namespace easynav + +#endif // EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp new file mode 100644 index 00000000..230ed320 --- /dev/null +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -0,0 +1,121 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. + +// #pragma once +#ifndef EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ +#define EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ + +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/path.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" + +namespace easynav +{ + +/// \brief A MPC parameters class. +class MPCParameters +{ +public: + MPCParameters( + Eigen::Vector2d goal, + Eigen::Vector3d x0, + Eigen::Vector3d theta0, + const pcl::PointCloud & points, + int N, + double dt); + + /// \brief Destructor. + ~MPCParameters(); + + Eigen::Vector2d goal; ///< Goal pose (x,y) to optimizer. + Eigen::Vector3d x0; ///< Init pos (x,y,z). + Eigen::Vector3d theta0; ///< Init orientation (roll,pitch,yaw). + const pcl::PointCloud & points; ///< Filtered Point Cloud to detect collisions. + + /// \brief Get the number of horizont steps + /// \return integer value of amount of horizont step used in optimization + int get_steps(); + + /// \brief Get the differential time used by integrator in kinematic model + /// \return double value of time in seconds + double get_timestep(); + + /// \brief Get angular cost used in angle optimization + /// \return double value of angular cost + double get_angular_tracking_cost(); + + /// \brief Get effort cost matrix used in optimization + /// \return Eigen::Matrix2d as effort cost matrix + Eigen::Matrix2d get_effort_cost(); + + /// \brief Get tracking cost matrix used in optimization + /// \return Eigen::Matrix2d as tracking cost matrix + Eigen::Matrix2d get_tracking_cost(); + + /// \brief Get smooth cost matrix used in optimization + /// \return Eigen::Matrix2d as smooth cost matrix + Eigen::Matrix2d get_smooth_cost(); + +private: + int N_ {5}; ///< Horizont Step to optimize. + double dt_ {0.1}; ///< Differential time to integrate. + Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost Matrix + Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost Matrix + Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost Matrix + double qtheta_ {3.0}; ///< Angular cost value. + +}; + +/// \brief A MPC Optimizer class. +class MPCOptimizer +{ +public: + MPCOptimizer(); + + /// \brief Destructor. + ~MPCOptimizer(); + + /// \brief Kinematic model for a particle used by optmizer to stimate final position + /// \param x Eigen::Vector3d Initial position for robot (x,y,z) + /// \param q Eigen::Vector3d Initial angel for robot (roll, pitch , yaw) + /// \param v double lineal velocity + /// \param w double angular velocity + /// \param dt double differential time used for integration + /// \return Eigen::Vector3d Final state for robot (x,y,yaw) + Eigen::Vector3d kinematic_model( + const Eigen::Vector3d & x, + const Eigen::Vector3d & q, double v, double w, double dt); + + /// \brief Wrap for real cost function + /// \param u std::vector Velocity vector to be optimized + /// \param grad std::vector gradient values for optimizer. It is NOT used for this implementation. + /// \param data MPCParameter pointer with parameters used by optimizer + /// \return double cost value used by nlOpt in internal callback + double cost_function(const std::vector & u, [[maybe_unused]] + std::vector & grad, void *data); + + /// \brief Real cost function with static propierties + /// \param u std::vector Velocity vector to be optimized + /// \param grad std::vector gradient values for optimizer. It is NOT used for this implementation. + /// \param data MPCParameter pointer with parameters used by optimizer + /// \return double cost value used by nlOpt in internal callback + static double nlopt_cost_callback( + const std::vector & x, + std::vector & grad, void *data); + +}; + +/// \brief Struct used as element in callback. +struct NLoptCallbackData +{ + MPCOptimizer *optimizer; ///< Pointer to optimizer. + MPCParameters *params; ///< Pointer to parameter for optimizer. +}; + +} // namespace easynav + +#endif // EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml new file mode 100644 index 00000000..c1ec0cb3 --- /dev/null +++ b/controllers/easynav_mpc_controller/package.xml @@ -0,0 +1,33 @@ + + + + easynav_mpc_controller + 0.0.1 + Easy Navigation: MPC Controller package. + Juan S. Cely G. + GPL-3.0-only + + ament_cmake + libnlopt-cxx-dev + libnlopt-dev + libnlopt0 + + easynav_common + easynav_core + easynav_system + pluginlib + tf2_ros + geometry_msgs + nav_msgs + + rclcpp_lifecycle + easynav_simple_common + std_srvs + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp new file mode 100644 index 00000000..66ec8eea --- /dev/null +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -0,0 +1,364 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// \file +/// \brief Implementation of the MPCController class. + +#include "easynav_mpc_controller/MPCController.hpp" +#include "easynav_system/GoalManager.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +namespace easynav +{ + +MPCController::MPCController() {} + +MPCController::~MPCController() = default; + +void +MPCController::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".horizon_steps", horizon_steps_); + node->declare_parameter(plugin_name + ".dt", dt_); + node->declare_parameter(plugin_name + ".safety_radius", safety_radius_); + node->declare_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); + node->declare_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + node->declare_parameter(plugin_name + ".verbose", verbose_); + + node->declare_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); + node->declare_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + + node->get_parameter(plugin_name + ".horizon_steps", horizon_steps_); + node->get_parameter(plugin_name + ".dt", dt_); + node->get_parameter(plugin_name + ".safety_radius", safety_radius_); + node->get_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); + node->get_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + node->get_parameter(plugin_name + ".verbose", verbose_); + + node->get_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); + node->get_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + + optimizer_ = std::make_unique(); + + mpc_path_pub_ = + node->create_publisher("/mpc/path", 10); + + detection_pub_ = + node->create_publisher("/mpc/detection", 10); +} + +void +MPCController::publish_mpc_path( + void *data, const std::vector & best_vel, + nav_msgs::msg::Path path) +{ + MPCParameters *params = reinterpret_cast(data); + nav_msgs::msg::Path mpc_path_; + + if (best_vel.size() > 0) { + mpc_path_.header.stamp = get_node()->now(); + mpc_path_.header.frame_id = path.header.frame_id; + for (size_t i = 0; i + 1 < best_vel.size(); i += 2) { + geometry_msgs::msg::PoseStamped pose_stamped; + double v = best_vel[i]; + double w = best_vel[i + 1]; + pose_stamped.header.frame_id = path.header.frame_id; + pose_stamped.header.stamp = path.header.stamp; + auto state = optimizer_->kinematic_model(params->x0, params->theta0, v, w, dt_); + pose_stamped.pose.position.x = state[0]; + pose_stamped.pose.position.y = state[1]; + mpc_path_.poses.push_back(pose_stamped); + } + mpc_path_pub_->publish(mpc_path_); + + } +} + +void +MPCController::collision_checker(void *data, std::vector & u) +{ + MPCParameters *params = reinterpret_cast(data); + double x_m = 0.0, y_m = 0.0, dist = 0.0, angle = 0.0; + size_t real_points = 0; + for (const auto & point : params->points) { + if(!std::isnan(point.x) || !std::isnan(point.y)) { + x_m += (point.x - params->x0[0]); + y_m += (point.y - params->x0[1]); + real_points++; + } + } + if(real_points != 0) { + x_m /= real_points; + y_m /= real_points; + dist = std::hypot(x_m, y_m); + angle = std::atan2(y_m, x_m) - params->theta0[2]; + if(dist < safety_radius_) { + if(!collision_state_) { + collision_state_ = true; + last_v_ = u[0]; + last_w_ = u[1]; + } + RCLCPP_WARN(get_node()->get_logger(), + "[COLLISION] Collision detected at: [%f] m and Theta: [%f] degrees", dist, + (angle * 180.00 / M_PI) ); + last_v_ = collision_factor_ * last_v_ * safety_radius_ / dist; + last_w_ = collision_factor_ * last_w_ * safety_radius_ / dist; + u[0] = -last_v_; + u[1] = -last_w_; + } else { + collision_state_ = false; + } + } +} + +void +MPCController::update_rt(NavState & nav_state) +{ + // If navigation is IDLE, force zero velocity + if (nav_state.has("navigation_state")) { + const auto nav_state_val = nav_state.get("navigation_state"); + if (nav_state_val == easynav::GoalManager::State::IDLE) { + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = 0.0; + cmd_vel_.twist.angular.z = 0.0; + nav_state.set("cmd_vel", cmd_vel_); + return; + } + } + + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("points")) { + if(verbose_) { + std::cout << "No Path, No Points or No Robot Pose" << std::endl; + } + return; + } + + nav_msgs::msg::Path path = nav_state.get("path"); + if (path.poses.empty()) { + // If the path is empty, stop the robot + cmd_vel_.header.frame_id = path.header.frame_id; + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = 0.0; + cmd_vel_.twist.angular.z = 0.0; + nav_state.set("cmd_vel", cmd_vel_); + return; + } + + // Build a local path that: + // 1) keeps only the segment that brings the robot closer to the goal, and + // 2) prepends a short straight segment from the robot pose to that segment. + const auto & robot_pose_msg = nav_state.get("robot_pose"); + const auto & robot_p = robot_pose_msg.pose.pose.position; + + // Goal is the last point of the planner path + const auto & goal_p = path.poses.back().pose.position; + + nav_msgs::msg::Path local_path; + local_path.header = path.header; + local_path.poses.clear(); + + // 1) Find the first index that actually brings us closer to the goal than our current pose + const double d_robot_goal = std::hypot(goal_p.x - robot_p.x, goal_p.y - robot_p.y); + std::size_t start_idx = 0; + for (std::size_t i = 0; i < path.poses.size(); ++i) { + const auto & pi = path.poses[i].pose.position; + const double d_pi_goal = std::hypot(goal_p.x - pi.x, goal_p.y - pi.y); + if (d_pi_goal <= d_robot_goal) { + start_idx = i; + break; + } + } + + // 2) Prepend a point at the robot position to ensure continuity from the current pose + geometry_msgs::msg::PoseStamped robot_ps; + robot_ps.header = path.header; + robot_ps.pose.position = robot_p; + robot_ps.pose.orientation = path.poses[start_idx].pose.orientation; + local_path.poses.push_back(robot_ps); + + // 3) Copy the remaining points from start_idx to the goal + for (std::size_t i = start_idx; i < path.poses.size(); ++i) { + local_path.poses.push_back(path.poses[i]); + } + + // Use the local path from now on + path = local_path; + + int num_elements = path.poses.size(); + size_t local_horizon; + if (num_elements > horizon_steps_) { + local_horizon = horizon_steps_; + } else { + local_horizon = num_elements - 1; + } + const auto & last_pose = path.poses[local_horizon].pose.position; + + const auto & perceptions = nav_state.get("points"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + const auto & filtered = PointPerceptionsOpsView(perceptions) + .filter({-2.0, -0.35, -1.0}, {0.0, 0.35, 1.0}) + .fuse(tf_info.map_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .collapse({NAN, NAN, 0.1}) + .downsample(0.1) + .as_points(); + + sensor_msgs::msg::PointCloud2 cloud_out; + pcl::toROSMsg(filtered, cloud_out); + cloud_out.header.frame_id = path.header.frame_id; + cloud_out.header.stamp = get_node()->now(); + detection_pub_->publish(cloud_out); + + const auto pose = nav_state.get("robot_pose").pose.pose; + double roll_, pitch_, yaw_; + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + tf2::Matrix3x3 m(q); + m.getRPY(roll_, pitch_, yaw_); + + // MPC Code + double minf; + std::vector u(2 * horizon_steps_, 0.0); + + auto params = MPCParameters( + Eigen::Vector2d(static_cast(last_pose.x), static_cast(last_pose.y)), + {pose.position.x, pose.position.y, pose.position.z}, + {roll_, pitch_, yaw_}, + filtered, + static_cast(horizon_steps_), + dt_); + + NLoptCallbackData cbdata{optimizer_.get(), ¶ms}; + + nlopt::opt opt(nlopt::LN_COBYLA, static_cast(u.size())); + opt.set_min_objective(easynav::MPCOptimizer::nlopt_cost_callback, &cbdata); + + std::vector lb(2 * horizon_steps_); + std::vector ub(2 * horizon_steps_); + for (int k = 0; k < horizon_steps_ ; k++) { + lb[2 * k] = -max_lin_vel_; + lb[2 * k + 1] = -max_ang_vel_; + ub[2 * k] = max_lin_vel_; + ub[2 * k + 1] = max_ang_vel_; + } + opt.set_lower_bounds(lb); + opt.set_upper_bounds(ub); + opt.set_xtol_rel(1e-3); + opt.set_ftol_rel(1e-3); + // opt.set_maxeval(1000); + + try { + nlopt::result result = opt.optimize(u, minf); + if(verbose_) { + if (result > 0) { + std::cerr << "Optimization Successful " << std::endl; + std::cout << "Result: " << result << std::endl; + } else { + std::cerr << "Optimization Unsuccessful " << std::endl; + } + } + + } catch (std::exception & e) { + std::cerr << "Optimization Error: " << e.what() << std::endl; + } + + if (ControllerMethodBase::collision_checker_active_) { + collision_checker(¶ms, u); + } + + // Final alignment phase with hysteresis on distance: + // - Enter when dist_to_goal <= 0.5 * pos_tol + // - Stay in this phase (even if dist grows slightly) until dist_to_goal > pos_tol + { + const auto & goal_pose = path.poses.back().pose; + + double pos_tol = fallback_goal_pos_tol_; + double yaw_tol = fallback_goal_yaw_tol_; + + if (nav_state.has("goal_tolerance.position")) { + pos_tol = nav_state.get("goal_tolerance.position"); + } + if (nav_state.has("goal_tolerance.yaw")) { + yaw_tol = nav_state.get("goal_tolerance.yaw"); + } + + const double dx_g = goal_pose.position.x - pose.position.x; + const double dy_g = goal_pose.position.y - pose.position.y; + const double dist_to_goal = std::hypot(dx_g, dy_g); + + const double yaw_goal = std::atan2( + 2.0 * (goal_pose.orientation.w * goal_pose.orientation.z + + goal_pose.orientation.x * goal_pose.orientation.y), + 1.0 - 2.0 * (goal_pose.orientation.y * goal_pose.orientation.y + + goal_pose.orientation.z * goal_pose.orientation.z)); + double e_theta_goal = std::atan2(std::sin(yaw_ - yaw_goal), std::cos(yaw_ - yaw_goal)); + + const double enter_dist = 0.5 * pos_tol; + + // Hysteresis based on distance to goal: start aligning when we are + // well inside the goal radius (enter_dist) and keep aligning until + // we move clearly outside (dist_to_goal > pos_tol). + const bool inside_hysteresis_band = (dist_to_goal <= pos_tol); + const bool should_enter_alignment = (dist_to_goal <= enter_dist); + + if ((inside_hysteresis_band && std::fabs(e_theta_goal) > yaw_tol) || + should_enter_alignment) + { + // Stay in place and rotate towards the goal orientation using a simple P controller + const double k_align = 1.0; + double vlin = 0.0; + double vrot = -k_align * e_theta_goal; + + vrot = std::clamp(vrot, -max_ang_vel_, max_ang_vel_); + + cmd_vel_.header.frame_id = path.header.frame_id; + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = vlin; + cmd_vel_.twist.angular.z = vrot; + + nav_state.set("cmd_vel", cmd_vel_); + publish_mpc_path(¶ms, u, path); + return; + } + } + + // Publish the computed velocity command + cmd_vel_.header.frame_id = path.header.frame_id; + cmd_vel_.header.stamp = get_node()->now(); + cmd_vel_.twist.linear.x = u[0]; + cmd_vel_.twist.angular.z = u[1]; + + nav_state.set("cmd_vel", cmd_vel_); + + // Publish the path + publish_mpc_path(¶ms, u, path); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::MPCController, easynav::ControllerMethodBase) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp new file mode 100644 index 00000000..123f1b3f --- /dev/null +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -0,0 +1,153 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// \file +/// \brief Implementation of MPCParameters and MPCOptimizer classes. + +#include "easynav_mpc_controller/MPCOptimizer.hpp" + +namespace easynav +{ + +MPCParameters::MPCParameters( + Eigen::Vector2d goal, + Eigen::Vector3d x0, + Eigen::Vector3d theta0, + const pcl::PointCloud & points, + int N, + double dt) +:goal(goal), x0(x0), theta0(theta0), points(points), N_(N), dt_(dt) {} + +MPCParameters::~MPCParameters() = default; + +int +MPCParameters::get_steps() +{ + return N_; +} + +double +MPCParameters::get_timestep() +{ + return dt_; +} + +double +MPCParameters::get_angular_tracking_cost() +{ + return qtheta_; +} + +Eigen::Matrix2d +MPCParameters::get_effort_cost() +{ + return R_; +} + +Eigen::Matrix2d +MPCParameters::get_tracking_cost() +{ + return Q_; +} + +Eigen::Matrix2d +MPCParameters::get_smooth_cost() +{ + return Rd_; +} + +MPCOptimizer::MPCOptimizer() {} + +MPCOptimizer::~MPCOptimizer() = default; + +Eigen::Vector3d +MPCOptimizer::kinematic_model( + const Eigen::Vector3d & x, const Eigen::Vector3d & q, double v, + double w, double dt) +{ + Eigen::Vector3d x_k1; + x_k1[0] = x[0] + v * cos(q[2]) * dt; + x_k1[1] = x[1] + v * sin(q[2]) * dt; + x_k1[2] = q[2] + w * dt; + return x_k1; +} + +double +MPCOptimizer::cost_function( + const std::vector & u, + [[maybe_unused]] std::vector & grad, void *data) +{ + MPCParameters *params = reinterpret_cast(data); + + Eigen::Vector3d position = params->x0; + Eigen::Vector3d orientation = params->theta0; + Eigen::Vector3d state; + int N = params->get_steps(); + double dt = params->get_timestep(); + double qtheta = params->get_angular_tracking_cost(); + double cost = 0.0; + + Eigen::Matrix2d R = params->get_effort_cost(); + Eigen::Matrix2d Q = params->get_tracking_cost(); + Eigen::Matrix2d Rd = params->get_smooth_cost(); + + for (int i = 0; i < N; ++i) { + double v = u[2 * i]; + double w = u[2 * i + 1]; + double dv, dw; + if(i < (N - 1)) { + dv = u[2 * (i + 1)] - u[2 * i]; + dw = u[2 * (i + 1) + 1] - u[2 * i + 1]; + } else { + dv = 0.0; + dw = 0.0; + } + + state = MPCOptimizer::kinematic_model(position, orientation, v, w, dt); + + Eigen::Vector2d pos = state.head<2>(); + Eigen::Vector2d error = params->goal - pos; + double error_theta = (atan2((error[1]), (error[0]))) - state[2]; + Eigen::Vector2d uk(v, w); + Eigen::Vector2d duk(dv, dw); + + // Tracking cost + cost += Q(0, 0) * error[0] * error[0] + Q(1, + 1) * error[1] * error[1] + qtheta * error_theta * error_theta; + // Effort Cost + cost += R(0, 0) * v * v + R(1, 1) * w * w; + // Smooth Cost + cost += Rd(0, 0) * dv * dv + Rd(1, 1) * dw * dw; + } + + return cost; + +} + +double +MPCOptimizer::nlopt_cost_callback( + const std::vector & x, + std::vector & grad, + void *data) +{ + auto *cbdata = static_cast(data); + return cbdata->optimizer->cost_function(x, grad, cbdata->params); +} + +} // namespace easynav diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index d702c4f5..4edc78f1 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_controller/CMakeLists.txt @@ -5,16 +5,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) +find_package(easynav_system REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io) @@ -32,7 +30,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core + easynav_system::easynav_system tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -75,6 +75,7 @@ ament_export_dependencies( easynav_core pluginlib tf2_ros + tf2_geometry_msgs geometry_msgs nav_msgs visualization_msgs diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 655e8568..b6d7f1e9 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -2,22 +2,27 @@ [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) - ## Description + A Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Jose Miguel Guerrero Hernandez ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_mppi_controller/MPPIController` - **Type:** `easynav::MPPIController` - **Base Class:** `easynav::ControllerMethodBase` @@ -25,8 +30,12 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig - **Description:** A Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_mppi_controller/MPPIController/...`. +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. + | Name | Type | Default | Description | |---|---|---:|---| | `.num_samples` | `int` | `100` | Number of trajectory rollouts per iteration. | @@ -40,21 +49,21 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.fov` | `double` | `M_PI/2.0` | Field of view used in trajectory sampling (radians). | | `.safety_radius` | `double` | `0.6` | Safety radius around the robot (meters). | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Publisher | `/mppi/candidates` | `visualization_msgs/msg/MarkerArray` | MPPI candidate trajectories as markers. | QoS depth=10 | | Publisher | `/mppi/optimal_path` | `visualization_msgs/msg/MarkerArray` | Optimal MPPI trajectory as markers. | QoS depth=10 | - ### Services -This package does not create service servers or clients. +This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `path` | `nav_msgs::msg::Path` | **Read** | Target path to track. | @@ -62,9 +71,10 @@ This package does not create service servers or clients. | `points` | `PointPerceptions` | **Read** | Perception point cloud(s) used for costs. | | `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Read** | Last commanded velocity (if provided in state). | - ## TF Frames + This controller does not explicitly publish or require TF frames in code. ## License + GPL-3.0-only diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp index ac53bfe2..4e34017e 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp @@ -8,22 +8,16 @@ #define EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ #include -#include #include -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" #include "easynav_mppi_controller/MPPIOptimizer.hpp" -#include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" namespace easynav { @@ -38,8 +32,8 @@ class MPPIController : public ControllerMethodBase ~MPPIController() override; /// \brief Initializes parameters and MPPI controller. - /// \return std::expected success or error message. - std::expected on_initialize() override; + /// \throws std::runtime_error if initialization fails. + void on_initialize() override; /// \brief Updates the controller using the given NavState. /// \param nav_state Current navigation state, including odometry and planned path. diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 7e9ee33a..672069e4 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -11,8 +11,10 @@ easynav_common easynav_core + easynav_system pluginlib tf2_ros + tf2_geometry_msgs geometry_msgs nav_msgs visualization_msgs diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index 8879738f..b471b039 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -20,13 +20,11 @@ /// \file /// \brief Implementation of the MPPIController class. -#include - -#include "tf2/utils.hpp" - #include "easynav_mppi_controller/MPPIController.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_system/GoalManager.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -37,7 +35,7 @@ MPPIController::MPPIController() {} MPPIController::~MPPIController() = default; -std::expected +void MPPIController::on_initialize() { auto node = get_node(); @@ -72,14 +70,13 @@ MPPIController::on_initialize() node->create_publisher("/mppi/candidates", 10); mppi_optimal_pub_ = node->create_publisher("/mppi/optimal_path", 10); - - return {}; } void MPPIController::publish_mppi_markers( const std::vector>> & all_trajs, const std::vector> & best_traj) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); visualization_msgs::msg::MarkerArray candidates; visualization_msgs::msg::MarkerArray optimal; int id = 0; @@ -87,7 +84,7 @@ void MPPIController::publish_mppi_markers( // Candidates in blue for (const auto & traj : all_trajs) { visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; + marker.header.frame_id = tf_info.map_frame; marker.header.stamp = rclcpp::Clock().now(); marker.ns = "mppi_candidates"; marker.id = id++; @@ -112,7 +109,7 @@ void MPPIController::publish_mppi_markers( // Best trajectory in red visualization_msgs::msg::Marker best_marker; - best_marker.header.frame_id = "map"; + best_marker.header.frame_id = tf_info.map_frame; best_marker.header.stamp = rclcpp::Clock().now(); best_marker.ns = "mppi_optimal_path"; best_marker.id = id++; @@ -143,11 +140,32 @@ void MPPIController::publish_mppi_markers( void MPPIController::update_rt(NavState & nav_state) { + // If navigation is IDLE, force zero velocity + if (nav_state.has("navigation_state")) { + const auto nav_state_val = nav_state.get("navigation_state"); + if (nav_state_val == easynav::GoalManager::State::IDLE) { + twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.twist.linear.x = 0.0; + twist_stamped_.twist.angular.z = 0.0; + nav_state.set("cmd_vel", twist_stamped_); + + // Also clear visualization markers when idle + visualization_msgs::msg::MarkerArray clear_markers; + visualization_msgs::msg::Marker delete_all; + delete_all.action = visualization_msgs::msg::Marker::DELETEALL; + clear_markers.markers.push_back(delete_all); + + mppi_candidates_pub_->publish(clear_markers); + mppi_optimal_pub_->publish(clear_markers); + return; + } + } + if (!nav_state.has("path") || !nav_state.has("robot_pose")) { return; } - const auto path = nav_state.get("path"); + const auto & path = nav_state.get("path"); if (path.poses.empty()) { // If the path is empty, stop the robot and clear markers @@ -167,12 +185,12 @@ MPPIController::update_rt(NavState & nav_state) return; } - const auto pose = nav_state.get("robot_pose").pose.pose; - const auto perceptions = nav_state.get("points"); - + const auto & pose = nav_state.get("robot_pose").pose.pose; + const auto & perceptions = nav_state.get("points"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) - .fuse("map") + .fuse(tf_info.map_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(0.1) diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp index 8205169f..7e5b9871 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp @@ -1,13 +1,10 @@ #include "easynav_mppi_controller/MPPIOptimizer.hpp" #include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include #include -#include - -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" namespace easynav { diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index 26541250..110edf69 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/controllers/easynav_serest_controller/CMakeLists.txt @@ -5,16 +5,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -29,6 +27,8 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core tf2_ros::tf2_ros + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -68,6 +68,8 @@ ament_export_dependencies( easynav_core pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs ) diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index f5d6fa49..8a36df77 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -4,20 +4,26 @@ ## Description + A SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---:| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_serest_controller/SerestController` - **Type:** `easynav::SerestController` - **Base Class:** `easynav::ControllerMethodBase` @@ -25,8 +31,12 @@ A SeReST (Smooth Error-Responsive Speed and Turning) controller for path trackin - **Description:** A SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_serest_controller/SerestController/...`. +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. + | Name | Type | Default | Description | |---|---|---:|---| | `.a_acc` | `double` | `0.8` | Comfortable forward acceleration (m/s²). | @@ -68,17 +78,18 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.v_progress_min` | `double` | `0.05` | Minimum forward speed to ensure progress (m/s). | | `.v_ref` | `double` | `0.6` | Nominal reference speed (m/s). | - ## Interfaces (Topics and Services) ### Subscriptions and Publications -This controller communicates through `NavState` (no direct ROS topics in this plugin). +This controller communicates through `NavState` (no direct ROS topics in this plugin). ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `path` | `nav_msgs::msg::Path` | **Read** | Reference path. | @@ -100,9 +111,10 @@ This package does not create service servers or clients. | `serest.debug.goal.in_final_align` | `double/int` | **Write** | Debug metric | | `serest.debug.goal.arrived` | `double/int` | **Write** | Debug metric | - ## TF Frames + This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). TF is not directly used in this plugin. ## License + GPL-3.0-only diff --git a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp index 9a22d258..0b908341 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -20,16 +20,13 @@ #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ -#include #include #include -#include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "tf2/utils.hpp" #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" @@ -65,9 +62,9 @@ class SerestController : public ControllerMethodBase /** * @brief Initialize parameters and internal state. - * @return std::expected Empty on success; error message otherwise. + * @throws std::runtime_error on initialization error. */ - std::expected on_initialize() override; + void on_initialize() override; /** * @brief Real-time control update (called ~20–30 Hz). @@ -185,7 +182,7 @@ class SerestController : public ControllerMethodBase * @return double Minimum obstacle distance (m). Infinity if none is found. */ double closest_obstacle_distance( - const NavState & nav_state) const; + const NavState & nav_state); /** * @brief Compute a safe linear speed bound from obstacle distance and slope. @@ -229,7 +226,7 @@ class SerestController : public ControllerMethodBase void safety_limits( const NavState & nav_state, const RefKinematics & rk, - double & d_closest, double & v_safe, double & v_curv) const; + double & d_closest, double & v_safe, double & v_curv); // Aplica corner‑guard: ajusta v_prog_ref y obtiene omega_boost + término ápice. void apply_corner_guard( @@ -357,6 +354,8 @@ class SerestController : public ControllerMethodBase double last_vrot_{0.0}; /// @brief Last update timestamp. rclcpp::Time last_update_ts_; + /// @brief Last input timestamp (max of path and odom). + rclcpp::Time last_input_ts_; /// @brief Output TwistStamped buffer. geometry_msgs::msg::TwistStamped twist_stamped_; }; diff --git a/controllers/easynav_serest_controller/package.xml b/controllers/easynav_serest_controller/package.xml index 0e39b126..76b4e613 100644 --- a/controllers/easynav_serest_controller/package.xml +++ b/controllers/easynav_serest_controller/package.xml @@ -12,6 +12,8 @@ easynav_common easynav_core pluginlib + tf2 + tf2_geometry_msgs tf2_ros geometry_msgs nav_msgs diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index e62f5162..b0fb4e0e 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -24,8 +24,11 @@ #include #include -#include "easynav_common/types/Perceptions.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + #include "easynav_common/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "easynav_serest_controller/SerestController.hpp" #include "pluginlib/class_list_macros.hpp" @@ -37,13 +40,13 @@ namespace easynav SerestController::SerestController() = default; SerestController::~SerestController() = default; -std::expected +void SerestController::on_initialize() { auto node = get_node(); const auto & ns = get_plugin_name(); - // Máximos y límites básicos + // Maximums and basic limits node->declare_parameter(ns + ".allow_reverse", allow_reverse_); node->declare_parameter(ns + ".v_progress_min", v_progress_min_); node->declare_parameter(ns + ".k_s_share_max", k_s_share_max_); @@ -53,7 +56,7 @@ SerestController::on_initialize() node->declare_parameter(ns + ".max_linear_acc", max_linear_acc_); node->declare_parameter(ns + ".max_angular_acc", max_angular_acc_); - // Seguimiento + // Tracking node->declare_parameter(ns + ".k_s", k_s_); node->declare_parameter(ns + ".k_theta", k_theta_); node->declare_parameter(ns + ".k_y", k_y_); @@ -61,7 +64,7 @@ SerestController::on_initialize() node->declare_parameter(ns + ".v_ref", v_ref_); node->declare_parameter(ns + ".eps", eps_); - // Seguridad + // Safety node->declare_parameter(ns + ".a_acc", a_acc_); node->declare_parameter(ns + ".a_brake", a_brake_); node->declare_parameter(ns + ".a_lat_max", a_lat_max_); @@ -70,7 +73,7 @@ SerestController::on_initialize() node->declare_parameter(ns + ".d_hard", d_hard_); node->declare_parameter(ns + ".t_emerg", t_emerg_); - // Blend en vértices + // Blend at vertices node->declare_parameter(ns + ".blend_base", blend_base_); node->declare_parameter(ns + ".blend_k_per_v", blend_k_per_v_); node->declare_parameter(ns + ".kappa_max", kappa_max_); @@ -145,8 +148,6 @@ SerestController::on_initialize() last_vlin_ = 0.0; last_vrot_ = 0.0; last_update_ts_ = node->now(); - - return {}; } SerestController::PathData @@ -216,29 +217,29 @@ SerestController::ref_heading_and_curvature( }; auto atan2dir = [](const Vec2 & t){return std::atan2(t.y, t.x);}; - // Índices de segmentos relevante: i-1, i, i + 1 + // Relevant segment indices: i-1, i, i + 1 const size_t i = prj.seg_idx; const Vec2 Ti = seg_dir(i); double psi_i = atan2dir(Ti); - // Mezcla local con previsualización + // Local blend with look-ahead double b = std::max(0.1, blend_base_ + blend_k_per_v_ * std::fabs(v_prev)); - // Determina si estamos cerca del comienzo o fin de un segmento + // Determine if we are near the beginning or end of a segment double s_i = pd.s_acc[i]; double s_ip1 = pd.s_acc[i + 1]; double s = prj.s_star; - // Por defecto: rumbo constante del segmento, kappa = 0 + // Default: constant segment heading, kappa = 0 rk.psi_ref = psi_i; rk.kappa_hat = 0.0; - // Blend cerca del inicio del segmento i (con i-1) + // Blend near the beginning of segment i (with i-1) if (i > 0 && (s - s_i) < b) { Vec2 Tim1 = seg_dir(i - 1); double psi_im1 = atan2dir(Tim1); - double w = 1.0 / (1.0 + std::exp(-( (s - s_i) / b ))); // sigmoide en [s_i, s_i+b] - // Interpolación circular de rumbos + double w = 1.0 / (1.0 + std::exp(-( (s - s_i) / b ))); // sigmoid in [s_i, s_i+b] + // Circular interpolation of headings double sx = (1.0 - w) * std::cos(psi_im1) + w * std::cos(psi_i); double sy = (1.0 - w) * std::sin(psi_im1) + w * std::sin(psi_i); rk.psi_ref = std::atan2(sy, sx); @@ -249,12 +250,12 @@ SerestController::ref_heading_and_curvature( rk.kappa_hat = std::clamp(dpsi * sigma_prime, -kappa_max_, kappa_max_); } - // Blend cerca del final del segmento i (hacia i + 1) + // Blend near the end of segment i (towards i + 1) if (i + 1 < pd.pts.size() - 1 && (s_ip1 - s) < b) { Vec2 Tip1 = seg_dir(i + 1); double psi_ip1 = atan2dir(Tip1); - double w = 1.0 / (1.0 + std::exp(-( (s_ip1 - s) / b ))); // simétrico - // Mezcla entre i y i + 1 + double w = 1.0 / (1.0 + std::exp(-( (s_ip1 - s) / b ))); // symmetric + // Blend between i and i + 1 double sx = (1.0 - w) * std::cos(psi_i) + w * std::cos(psi_ip1); double sy = (1.0 - w) * std::sin(psi_i) + w * std::sin(psi_ip1); rk.psi_ref = std::atan2(sy, sx); @@ -262,7 +263,7 @@ SerestController::ref_heading_and_curvature( double dpsi = std::atan2(std::sin(psi_ip1 - psi_i), std::cos(psi_ip1 - psi_i)); double sigma_prime = (w * (1 - w)) / b; double kappa2 = std::clamp(dpsi * sigma_prime, -kappa_max_, kappa_max_); - // Si hay dos blends solapados, combinamos suavemente (promedio) + // If there are two overlapping blends, combine smoothly (average) rk.kappa_hat = 0.5 * (rk.kappa_hat + kappa2); } @@ -275,40 +276,59 @@ SerestController::frenet_errors( const Projection & prj, double psi_ref, double & e_y, double & e_theta) const { - // Vector normal a la derecha de la tangente + // Normal vector to the right of the tangent Vec2 T = v2(std::cos(psi_ref), std::sin(psi_ref)); - Vec2 N = v2(-T.y, T.x); // 90º a la izquierda (convención) + Vec2 N = v2(-T.y, T.x); // 90º to the left (convention) Vec2 err = robot_xy - prj.closest; - e_y = dot(err, N); // distancia lateral con signo + e_y = dot(err, N); // signed lateral distance e_theta = std::atan2(std::sin(robot_yaw - psi_ref), std::cos(robot_yaw - psi_ref)); } double SerestController::closest_obstacle_distance( - const NavState & nav_state) const + const NavState & nav_state) { - // 1) Preferir medición directa si existe + // 1) Prefer direct measurement if it exists if (nav_state.has("closest_obstacle_distance")) { try { return nav_state.get("closest_obstacle_distance"); } catch (...) { - // continúa a estimación + // fall through to estimation } } - // 2) Analizar los sensores de distancia + // 2) Analyze distance sensors if (!nav_state.has("points")) {return std::numeric_limits::infinity();} - const auto perceptions = nav_state.get("points"); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(0.3) - .fuse(get_tf_prefix() + "base_link") - .filter({-dist_search_radius_, -dist_search_radius_, NAN}, - {dist_search_radius_, dist_search_radius_, 2.0}) - .collapse({NAN, NAN, 0.1}) - .downsample(0.3) - .as_points(); + const auto & perceptions = nav_state.get("points"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(0.3) + .fuse(tf_info.robot_frame) + .filter({-dist_search_radius_, -dist_search_radius_, NAN}, + {dist_search_radius_, dist_search_radius_, 2.0}) + .collapse({NAN, NAN, 0.1}) + .downsample(0.3); + const auto & fused = view.as_points(); + + auto latest_time = [](const PointPerceptions & perceptions){ + rclcpp::Time latest_stamp; + bool inited = false; + + for (const auto & perception : perceptions) { + if (!inited || perception->stamp > latest_stamp) { + latest_stamp = perception->stamp; + inited = true; + } + } + return latest_stamp; + }; + + if (last_input_ts_ < latest_time(perceptions)) { + last_input_ts_ = latest_time(perceptions); + } double min_dist = std::numeric_limits::infinity(); for (const auto p : fused) { @@ -322,7 +342,7 @@ SerestController::closest_obstacle_distance( double SerestController::v_safe_from_distance(double d, double slope_sin) const { - // a_eff reduce la capacidad de frenado en pendiente; en 2D slope_sin = 0 + // a_eff reduces braking capability on slopes; in 2D slope_sin = 0 const double a_eff = std::max(0.1, a_brake_ - 9.81 * slope_sin); if (d <= d0_margin_) {return 0.0;} @@ -335,7 +355,7 @@ SerestController::v_safe_from_distance(double d, double slope_sin) const double SerestController::v_curvature_limit(double kappa_hat) const { - // Tres límites típicos: v_max, ω_max/|κ| y sqrt(a_lat_max/|κ|) + // Three typical limits: v_max, ω_max/|κ| and sqrt(a_lat_max/|κ|) double v1 = max_linear_speed_; double v2 = std::numeric_limits::infinity(); double v3 = std::numeric_limits::infinity(); @@ -364,14 +384,23 @@ SerestController::fetch_required_inputs( nav_msgs::msg::Path & path, nav_msgs::msg::Odometry & odom) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { - publish_stop(nav_state, "base_link"); + publish_stop(nav_state, tf_info.robot_frame); return false; } path = nav_state.get("path"); odom = nav_state.get("robot_pose"); + if (rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { + last_input_ts_ = path.header.stamp; + } + if (rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { + last_input_ts_ = odom.header.stamp; + } + if (path.poses.empty()) { publish_stop(nav_state, path.header.frame_id); return false; @@ -399,10 +428,6 @@ SerestController::compute_goal_zone( double & stop_r, double & slow_r, double & gamma_slow, Vec2 & goal_xy, double & yaw_goal) const { - const double PI = 3.14159265358979323846; - const double goal_yaw_tol = goal_yaw_tol_deg_ * PI / 180.0; // (no se devuelve, pero útil si quieres) - (void)goal_yaw_tol; - const auto & pgoal = path.poses.back().pose.position; yaw_goal = tf2::getYaw(path.poses.back().pose.orientation); goal_xy = Vec2{pgoal.x, pgoal.y}; @@ -410,6 +435,9 @@ SerestController::compute_goal_zone( dist_xy_goal = std::hypot(robot_xy.x - goal_xy.x, robot_xy.y - goal_xy.y); e_theta_goal = std::atan2(std::sin(robot_yaw - yaw_goal), std::cos(robot_yaw - yaw_goal)); + // Use externally provided goal position tolerance when available (via update_rt), + // which stores it in goal_pos_tol_. Here we only shape the radial zone using + // the current value of goal_pos_tol_ as stop radius. stop_r = std::max(0.0, goal_pos_tol_); slow_r = std::max(slow_radius_, stop_r + 0.05); @@ -423,12 +451,12 @@ void SerestController::safety_limits( const NavState & nav_state, const RefKinematics & rk, - double & d_closest, double & v_safe, double & v_curv) const + double & d_closest, double & v_safe, double & v_curv) { d_closest = closest_obstacle_distance(nav_state); v_safe = v_safe_from_distance(d_closest, /*slope_sin=*/0.0); - // Límite por curvatura (versión “soft” del archivo original) + // Curvature-based limit ("soft" version of the original file) const double ak = std::fabs(rk.kappa_hat); double v_curv_soft = std::numeric_limits::infinity(); if (ak > 1e-6) { @@ -463,7 +491,7 @@ SerestController::apply_corner_guard( alpha_corner = std::clamp(1.0 / (1.0 + penal), corner_min_alpha_, 1.0); omega_boost = 1.0 + corner_boost_omega_ * std::min(1.0, e_y_out / std::max(1e-3, ell_)); - // Empuje al “ápice” si vas por fuera y hay curvatura + // Push towards the "apex" if you are outside and there is curvature if (std::fabs(rk.kappa_hat) > 1e-6 && e_y_out > 0.0) { const double e_y_des = -sgn_k * std::max(0.0, apex_ey_des_); const double e_y_err = e_y - e_y_des; @@ -478,17 +506,16 @@ SerestController::should_turn_in_place( bool allow_reverse, double e_theta, double e_theta_goal, double dist_to_end, [[maybe_unused]] double turn_in_place_thr) const { - // Mantenemos compatibilidad con la firma, pero ignoramos turn_in_place_thr - // y usamos dos umbrales internos sin exponer parámetros. - const double PI = 3.14159265358979323846; - const double thr_enter = 60.0 * PI / 180.0; // entra a girar si |e_theta| > 60° - // const double thr_exit = 35.0 * PI / 180.0; // sale de girar si |e_theta| < 35° - - // No permitimos “atajo” marcha atrás en esta decisión: si no permites reverse, - // el criterio es más estricto. + // Keep compatibility with the signature, but ignore turn_in_place_thr + // and use two internal thresholds without exposing parameters. + const double thr_enter = 60.0 * M_PI / 180.0; // enter TiP if |e_theta| > 60° + // const double thr_exit = 35.0 * PI / 180.0; // exit TiP if |e_theta| < 35° + + // Do not allow reverse "shortcut" in this decision: if reverse is not allowed, + // the criterion is stricter. bool tip = (!allow_reverse) && (std::fabs(e_theta) > thr_enter); - // Cerca del final, si el yaw al objetivo es grande, también pedimos TiP + // Near the end, if the yaw error to the goal is large, also request TiP if (dist_to_end < 0.50) { if (!allow_reverse && std::fabs(e_theta_goal) > thr_enter) { tip = true; @@ -504,8 +531,7 @@ SerestController::maybe_final_align_and_publish( double dist_xy_goal, double stop_r, double e_theta_goal, double gamma_slow, double dt) { - const double PI = 3.14159265358979323846; - const double goal_yaw_tol = goal_yaw_tol_deg_ * PI / 180.0; + const double goal_yaw_tol = goal_yaw_tol_deg_ * M_PI / 180.0; if (dist_xy_goal > stop_r) { return false; @@ -533,7 +559,7 @@ SerestController::maybe_final_align_and_publish( // Publicación y actualización de estado twist_stamped_.header.frame_id = path.header.frame_id; - twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.header.stamp = last_input_ts_; twist_stamped_.twist.linear.x = vlin; twist_stamped_.twist.angular.z = vrot; @@ -576,7 +602,7 @@ SerestController::publish_cmd_and_debug( int in_final_align, int arrived) { twist_stamped_.header.frame_id = path.header.frame_id; - twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.header.stamp = last_input_ts_; twist_stamped_.twist.linear.x = vlin; twist_stamped_.twist.angular.z = vrot; nav_state.set("cmd_vel", twist_stamped_); @@ -599,7 +625,7 @@ SerestController::publish_cmd_and_debug( void SerestController::publish_stop(NavState & nav_state, const std::string & frame_id) { - auto now = get_node()->now(); + auto now = last_input_ts_; twist_stamped_.header.frame_id = frame_id; twist_stamped_.header.stamp = now; twist_stamped_.twist.linear.x = 0.0; @@ -618,6 +644,16 @@ SerestController::update_rt(NavState & nav_state) nav_msgs::msg::Odometry odom; if (!fetch_required_inputs(nav_state, path, odom)) {return;} + // 1.5) Goal tolerances: prefer shared GoalManager values, fallback to local params + double goal_pos_tol = goal_pos_tol_; + double goal_yaw_tol = goal_yaw_tol_deg_ * (M_PI / 180.0); + if (nav_state.has("goal_tolerance.position")) { + goal_pos_tol = nav_state.get("goal_tolerance.position"); + } + if (nav_state.has("goal_tolerance.yaw")) { + goal_yaw_tol = nav_state.get("goal_tolerance.yaw"); + } + // 2) Robot state (position + yaw) Vec2 robot_xy; double yaw = 0.0; robot_state_from_odom(odom, robot_xy, yaw); @@ -643,13 +679,12 @@ SerestController::update_rt(NavState & nav_state) // 6.5) Early turn-in-place with hysteresis (start-of-path aware) { - const double PI = 3.14159265358979323846; const double s_total = pd.s_acc.back(); const double dist_to_end = s_total - prj.s_star; // Internal angular thresholds (enter/exit) - const double thr_enter = 60.0 * PI / 180.0; - const double thr_exit = 35.0 * PI / 180.0; + const double thr_enter = 60.0 * M_PI / 180.0; + const double thr_exit = 35.0 * M_PI / 180.0; const double near_start_s = 0.30; // treat the first 30 cm as the start region // Base request from the regular criterion (using the provided threshold) @@ -712,8 +747,7 @@ SerestController::update_rt(NavState & nav_state) double v_prog_ref = v_prog_ref_free * gamma_slow; // Maintain a small cruising speed when roughly aligned and outside the stop zone (no reverse) - const double PI = 3.14159265358979323846; - const double align_thr = 30.0 * PI / 180.0; + const double align_thr = 30.0 * M_PI / 180.0; if (!allow_reverse_ && (dist_xy_goal > stop_r) && std::fabs(e_theta) < align_thr) { v_prog_ref = std::max(v_prog_ref, std::min(slow_min_speed_, v_prog_ref_free)); } @@ -764,7 +798,7 @@ SerestController::update_rt(NavState & nav_state) // Optional classic TiP safeguard using the same angular threshold { - const double turn_in_place_thr = (60.0 * PI / 180.0); + const double turn_in_place_thr = (60.0 * M_PI / 180.0); const double s_total = pd.s_acc.back(); const double dist_to_end = s_total - prj.s_star; diff --git a/controllers/easynav_simple_controller/CMakeLists.txt b/controllers/easynav_simple_controller/CMakeLists.txt index c3346d89..4bd163a9 100644 --- a/controllers/easynav_simple_controller/CMakeLists.txt +++ b/controllers/easynav_simple_controller/CMakeLists.txt @@ -5,16 +5,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -30,6 +28,8 @@ target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common easynav_core::easynav_core tf2_ros::tf2_ros + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs pluginlib::pluginlib ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -70,6 +70,8 @@ ament_export_dependencies( easynav_core pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs ) diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index c01e2eb2..e9ae782d 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -6,12 +6,16 @@ Simple path-following controller that uses PID controllers and a look-ahead reference pose to follow a planned path. It produces velocity commands (`cmd_vel`) based on the reference pose sampled at a look-ahead distance and limits linear/angular speeds and accelerations. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---:| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | @@ -24,42 +28,49 @@ Simple path-following controller that uses PID controllers and a look-ahead refe - **Description:** Path-following controller using PID (linear and angular) and a look-ahead strategy. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_simple_controller/SimpleController/...`. +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. + | Name | Type | Default | Description | |---|---|---:|---| -| `max_linear_speed` | `double` | `1.0` | Maximum linear speed (m/s). -| `max_angular_speed` | `double` | `1.0` | Maximum angular speed (rad/s). -| `max_linear_acc` | `double` | `0.3` | Maximum linear acceleration (m/s²). -| `max_angular_acc` | `double` | `0.3` | Maximum angular acceleration (rad/s²). -| `look_ahead_dist` | `double` | `1.0` | Look-ahead distance to sample the reference pose on the path (m). -| `tolerance_dist` | `double` | `0.05` | Distance threshold to switch to pure orientation tracking (m). -| `final_goal_angle_tolerance` | `double` | `0.1` | Angular tolerance (rad) used to decide final-goal arrival. -| `k_rot` | `double` | `0.5` | Gain used to reduce linear speed based on angular velocity (higher: stronger reduction while turning). -| `linear_kp` | `double` | `0.95` | Proportional gain for the linear PID controller. -| `linear_ki` | `double` | `0.03` | Integral gain for the linear PID controller. -| `linear_kd` | `double` | `0.08` | Derivative gain for the linear PID controller. -| `angular_kp` | `double` | `1.5` | Proportional gain for the angular PID controller. -| `angular_ki` | `double` | `0.03` | Integral gain for the angular PID controller. -| `angular_kd` | `double` | `0.08` | Derivative gain for the angular PID controller. +| `max_linear_speed` | `double` | `1.0` | Maximum linear speed (m/s). | +| `max_angular_speed` | `double` | `1.0` | Maximum angular speed (rad/s). | +| `max_linear_acc` | `double` | `0.3` | Maximum linear acceleration (m/s²). | +| `max_angular_acc` | `double` | `0.3` | Maximum angular acceleration (rad/s²). | +| `look_ahead_dist` | `double` | `1.0` | Look-ahead distance to sample the reference pose on the path (m). | +| `tolerance_dist` | `double` | `0.05` | Distance threshold to switch to pure orientation tracking (m). | +| `final_goal_angle_tolerance` | `double` | `0.1` | Angular tolerance (rad) used to decide final-goal arrival. | +| `k_rot` | `double` | `0.5` | Gain used to reduce linear speed based on angular velocity (higher: stronger reduction while turning). | +| `linear_kp` | `double` | `0.95` | Proportional gain for the linear PID controller. | +| `linear_ki` | `double` | `0.03` | Integral gain for the linear PID controller. | +| `linear_kd` | `double` | `0.08` | Derivative gain for the linear PID controller. | +| `angular_kp` | `double` | `1.5` | Proportional gain for the angular PID controller. | +| `angular_ki` | `double` | `0.03` | Integral gain for the angular PID controller. | +| `angular_kd` | `double` | `0.08` | Derivative gain for the angular PID controller. | ## Interfaces (NavState, Topics and Services) ### NavState + This controller uses the shared `NavState` bag provided by the `easynav_core` framework. The following keys are used at runtime by `SimpleController`: | Key | Type | Access | Notes | |---|---|---|---| -| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot odometry used to compute the robot pose and yaw. -| `path` | `nav_msgs::msg::Path` | **Read** | Planned path to follow. The controller samples a reference pose at `look_ahead_dist` along this path. -| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Write** | Output velocity command. Header.frame_id is set to `path.header.frame_id` when available and stamp to the controller node clock. +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot odometry used to compute the robot pose and yaw. | +| `path` | `nav_msgs::msg::Path` | **Read** | Planned path to follow. The controller samples a reference pose at `look_ahead_dist` along this path. | +| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Write** | Output velocity command. Header.frame_id is set to `path.header.frame_id` when available and stamp to the controller node clock. | ### Topics / Services -The controller itself does not create ROS publishers/subscribers or service servers. It interacts via the `NavState` abstraction; how `NavState` is exposed (topics or other IPC mechanisms) depends on the integrating node. +The controller itself does not create ROS publishers/subscribers or service servers. It interacts via the `NavState` abstraction; how `NavState` is exposed (topics or other IPC mechanisms) depends on the integrating node. ## TF Frames + This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). TF is not directly used in this plugin. ## License -GPL-3.0-only \ No newline at end of file + +GPL-3.0-only diff --git a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp index e4cb072b..8820859d 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp @@ -8,10 +8,8 @@ #define EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ #include -#include #include -#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" @@ -33,8 +31,8 @@ class SimpleController : public ControllerMethodBase ~SimpleController() override; /// \brief Initializes parameters and PID controllers. - /// \return std::expected success or error message. - std::expected on_initialize() override; + /// \throws std::runtime_error on initialization error. + void on_initialize() override; /// \brief Updates the controller using the given NavState. /// \param nav_state Current navigation state, including odometry and planned path. diff --git a/controllers/easynav_simple_controller/package.xml b/controllers/easynav_simple_controller/package.xml index 1ed295db..8eadc469 100644 --- a/controllers/easynav_simple_controller/package.xml +++ b/controllers/easynav_simple_controller/package.xml @@ -12,6 +12,8 @@ easynav_common easynav_core pluginlib + tf2 + tf2_geometry_msgs tf2_ros geometry_msgs nav_msgs diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp index b89f2a88..9438c50f 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -20,9 +20,8 @@ /// \file /// \brief Implementation of the SimpleController class. -#include - #include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "easynav_simple_controller/SimpleController.hpp" @@ -38,7 +37,7 @@ SimpleController::SimpleController() SimpleController::~SimpleController() = default; -std::expected +void SimpleController::on_initialize() { auto node = get_node(); @@ -86,8 +85,6 @@ SimpleController::on_initialize() last_vlin_ = 0.0; last_vrot_ = 0.0; last_update_ts_ = node->now(); - - return {}; } @@ -100,7 +97,7 @@ SimpleController::update_rt(NavState & nav_state) if (!nav_state.has("path")) {return;} if (!nav_state.has("robot_pose")) {return;} - const auto path = nav_state.get("path"); + const auto & path = nav_state.get("path"); if (path.poses.empty()) { twist_stamped_.header.frame_id = path.header.frame_id; @@ -115,7 +112,7 @@ SimpleController::update_rt(NavState & nav_state) } // If we're very close to the final path pose, stop the robot. - const auto pose = nav_state.get("robot_pose").pose.pose; + const auto & pose = nav_state.get("robot_pose").pose.pose; const auto & goal_pose = path.poses.back().pose; double dist_to_goal = get_distance(pose, goal_pose); double angle_to_goal = get_diff_angle(pose.orientation, goal_pose.orientation); diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index af52d97e..4af0d5c2 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -144,6 +144,13 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) { auto node = std::make_shared("test_savemap_node"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test_savemap"); auto static_map = std::make_shared(); diff --git a/controllers/easynav_vff_controller/CMakeLists.txt b/controllers/easynav_vff_controller/CMakeLists.txt index 131de417..04a2da17 100644 --- a/controllers/easynav_vff_controller/CMakeLists.txt +++ b/controllers/easynav_vff_controller/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index ab2314ce..299a1958 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -1,28 +1,62 @@ # easynav_vff_controller -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description -Easy Navigation: VFF Controller package. + +Vector Field Histogram (VFF) style local obstacle avoidance controller. Generates `cmd_vel` commands from proximity/cost data and a target path reference using a histogram-based steering selection. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Jose Miguel Guerrero Hernandez ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_vff_controller/VffController` **Type:** `easynav::VffController` - **Base:** `easynav::ControllerMethodBase` - **Library:** `vff_controller` - **Description:** A VFF implementation for the Controller Method. + **Base Class:** `easynav::ControllerMethodBase` + **Library:** `easynav_vff_controller` + **Description:** Histogram-based obstacle avoidance controller producing velocity commands. + +## Parameters + +No ROS parameters are currently declared in code. (Add declarations in the plugin to enable runtime tuning.) + +> This plugin derives from [`easynav::ControllerMethodBase`](https://github.com/EasyNavigation/EasyNavigation/tree/rolling/easynav_core#easynavcontrollermethodbase). \ +> See that section for shared collision-checking parameters and debug markers common to all controllers. +## Interfaces +### NavState Keys + +| Key | Type | Access | Notes | +|---|---|---|---| +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current pose for steering decisions. | +| `path` | `nav_msgs::msg::Path` | **Read** | Planned path reference. | +| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Write** | Output velocity command. | + +### Publications + +| Topic | Type | Purpose | QoS | +|---|---|---|---| +| `/vff/markers` | `visualization_msgs/msg/MarkerArray` | Histogram / debug visualization (name inferred from code marker publisher variable). | depth=10 | + +### Subscriptions / Services + +None directly; relies on NavState for data sharing. + +## TF Frames + +Relies on frames stamped in `robot_pose`; does not query TF directly. ## License + GPL-3.0-only diff --git a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp index 41a2c855..f9048de6 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -23,12 +23,9 @@ #ifndef EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ #define EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ -#include - #include "pcl/point_cloud.h" #include "easynav_core/ControllerMethodBase.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" @@ -84,11 +81,9 @@ class VffController : public easynav::ControllerMethodBase * This method is called once during the configuration phase of the controller node, * and can be optionally overridden by derived classes to perform custom setup logic. * - * @return std::expected Returns an expected object: - * - `void` if initialization was successful, - * - a `std::string` containing an error message if initialization failed. + * @throws std::runtime_error on initialization error. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the localization estimate based on the current navigation state. diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 5e5568b5..594ae7c4 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -20,15 +20,12 @@ /// \file /// \brief Implementation of the VffController class. -#include - #include "easynav_vff_controller/VffController.hpp" #include "easynav_common/types/NavState.hpp" #include "easynav_common/types/PointPerception.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/goals.hpp" -#include "nav_msgs/msg/path.hpp" #include #include @@ -36,7 +33,7 @@ namespace easynav { -std::expected VffController::on_initialize() +void VffController::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -63,9 +60,11 @@ std::expected VffController::on_initialize() node->get_parameter(plugin_name + ".max_speed", max_speed_); node->get_parameter(plugin_name + ".max_angular_speed", max_angular_speed_); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + // Initialize the odometry message cmd_vel_.header.stamp = node->now(); - cmd_vel_.header.frame_id = get_tf_prefix() + "base_link"; + cmd_vel_.header.frame_id = tf_info.map_frame; cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.linear.y = 0.0; cmd_vel_.twist.linear.z = 0.0; @@ -76,8 +75,6 @@ std::expected VffController::on_initialize() // Publisher for visualization markers marker_array_pub_ = node->create_publisher( "vff/markers_vff", 10); - - return {}; } visualization_msgs::msg::MarkerArray @@ -214,9 +211,10 @@ void VffController::update_rt(NavState & nav_state) if (!nav_state.has("robot_pose")) {return;} const auto & all_goals = nav_state.get("goals"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (all_goals.goals.empty()) { - cmd_vel_.header.frame_id = get_tf_prefix() + "map"; + cmd_vel_.header.frame_id = tf_info.map_frame; cmd_vel_.header.stamp = get_node()->now(); cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.angular.z = 0.0; @@ -258,18 +256,20 @@ void VffController::update_rt(NavState & nav_state) // Calculate the angle error double angle_error = normalize_angle(bearing - yaw); - const auto perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get("points"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0}) - .fuse(get_tf_prefix() + "base_link") + .fuse(tf_info.map_frame) .filter({obstacle_detection_x_min_, obstacle_detection_y_min_, obstacle_detection_z_min_}, {obstacle_detection_x_max_, obstacle_detection_y_max_, - obstacle_detection_z_max_}).as_points(); + obstacle_detection_z_max_}) + .as_points(); // Get VFF vectors - const VFFVectors & vff = get_vff(angle_error, fused, get_tf_prefix() + "base_link"); + const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_frame); // Use result vector to calculate output speed const auto & v = vff.result; diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index 1ea83e63..aabb1b11 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_costmap_common REQUIRED) diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8d3ae420..8e5f5e7b 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -1,21 +1,25 @@ # easynav_costmap_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for prediction. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_costmap_localizer/AMCLLocalizer` - **Type:** `easynav::AMCLLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,6 +27,7 @@ AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for predi - **Description:** AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for prediction. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_costmap_localizer/AMCLLocalizer/...`. | Name | Type | Default | Description | @@ -41,33 +46,35 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.min_noise_yaw` | `double` | `0.05` | Minimum yaw noise (rad). | | `.compute_odom_from_tf` | `bool` | `false` | If true, read odometry from TF (odom->base_footprint) instead of /odom topic. | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Subscription | `/odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false. | SensorDataQoS (reliable) | +| Subscription | `odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false. | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Init particles pose to the received pose, using covariance | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set. | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Estimated pose with covariance. | depth=10 | - ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | | `map.static` | `Costmap2D` | **Read** | Static costmap for likelihood evaluation. | - ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map -> odom` | Aligns the odometry frame with the map frame. | | Requires (optional) | `odom -> base_footprint` | Required when compute_odom_from_tf=true (read from TF). | - ## License + GPL-3.0-only diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp index 19b81a81..44b8352b 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -24,11 +24,6 @@ #define EASYNAV_COSTMAP_LOCALIZER__AMCLLOCALIZER_HPP_ #include -#include -#include -#include -#include -#include #include #include @@ -72,9 +67,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error on initialization error. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -164,6 +159,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -171,6 +169,14 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + + /** * @brief Update odom from TFs instead of a odom topic * @@ -181,7 +187,7 @@ class AMCLLocalizer : public LocalizerMethodBase std::vector particles_; /// Random number generator used for sampling noise. - std::default_random_engine rng_; + std::mt19937 rng_; /// Current estimated odometry-based pose. nav_msgs::msg::Odometry pose_; @@ -216,6 +222,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Time interval (in seconds) after which the particles should be reseeded. double reseed_time_; + /// Timestamp of the last input message (odometry or initial pose). + rclcpp::Time last_input_time_; + /// Timestamp of the last reseed event. rclcpp::Time last_reseed_; }; diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 48cd58c1..48bc71ea 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -20,14 +20,12 @@ /// \file /// \brief Implementation of the AMCLLocalizer class using Costmap2D. -#include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -162,6 +160,7 @@ using namespace std::chrono_literals; AMCLLocalizer::AMCLLocalizer() +: rng_(std::random_device{}()) { NavState::register_printer( [](const nav_msgs::msg::Odometry & odom) { @@ -178,7 +177,8 @@ AMCLLocalizer::AMCLLocalizer() double roll, pitch, yaw; tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")"; + ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << " } Odometry with pose: (x: " << + x << ", y: " << y << ", yaw: " << yaw << ")"; return ret.str(); }); } @@ -187,7 +187,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +void AMCLLocalizer::on_initialize() { auto node = get_node(); @@ -266,11 +266,13 @@ AMCLLocalizer::on_initialize() estimate_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); + last_reseed_ = get_node()->now(); + last_input_time_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - - return {}; } void printTransform(const tf2::Transform & tf) @@ -319,6 +321,7 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) if (compute_odom_from_tf_) {return;} tf2::fromMsg(msg->pose.pose, odom_); + last_input_time_ = msg->header.stamp; if (!initialized_odom_) { last_odom_ = odom_; @@ -326,13 +329,169 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) } } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + // Check expected frame + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; // C(0,0) + double cov_xy = cov[1]; // C(0,1) + double var_y = cov[7]; // C(1,1) + + // Yaw variance + double var_yaw = cov[35]; // C(5,5) + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = + std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, yaw_stddev); + + const std::size_t N = particles_.size(); + + for (std::size_t i = 0; i < N; ++i) { + // Sample a correlated 2D perturbation according to covariance(x,y) + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + + // Sample yaw + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights as safety measure + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + + void AMCLLocalizer::update_odom_from_tf() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); + tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "AMCLLocalizer::update: TF failed: %s", ex.what()); return; @@ -343,8 +502,11 @@ AMCLLocalizer::update_odom_from_tf() last_odom_ = odom_; odom_ = tf_odom; + last_input_time_ = tf_msg.header.stamp; initialized_odom_ = true; + + } void @@ -372,8 +534,6 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) tf2::Matrix3x3(delta.getRotation()).getRPY(roll, pitch, yaw); double rot_len = std::abs(yaw); - std::random_device rd; - std::mt19937 gen(rd()); for (auto & p : particles_) { std::normal_distribution noise_dx(0.0, std::abs(dx) * noise_translation_); @@ -385,11 +545,11 @@ AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); tf2::Vector3 noisy_translation( - dx + noise_dx(gen), - dy + noise_dy(gen), - dz + noise_dz(gen)); + dx + noise_dx(rng_), + dy + noise_dy(rng_), + dz + noise_dz(rng_)); - double noisy_yaw = yaw + noise_yaw(gen); + double noisy_yaw = yaw + noise_yaw(rng_); tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_yaw); @@ -415,7 +575,7 @@ AMCLLocalizer::correct(NavState & nav_state) return; } - const auto perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get("points"); if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); @@ -424,19 +584,36 @@ AMCLLocalizer::correct(NavState & nav_state) const auto & map_static = nav_state.get("map.static"); - const auto & filtered = PointPerceptionsOpsView(perceptions) - .downsample(map_static.getResolution()) - .fuse(get_tf_prefix() + "base_footprint") - .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .collapse({NAN, NAN, 0.1}) - .downsample(map_static.getResolution()) - .as_points(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(map_static.getResolution()) + .fuse(tf_info.robot_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .collapse({NAN, NAN, 0.1}) + .downsample(map_static.getResolution()); + const auto & filtered = view.as_points(); if (filtered.empty()) { RCLCPP_WARN(get_node()->get_logger(), "No points to correct"); return; } + auto latest_time = [](const PointPerceptions & perceptions){ + rclcpp::Time latest_stamp; + bool inited = false; + + for (const auto & perception : perceptions) { + if (!inited || perception->stamp > latest_stamp) { + latest_stamp = perception->stamp; + inited = true; + } + } + return latest_stamp; + }; + + last_input_time_ = latest_time(perceptions); + for (auto & particle : particles_) { int hits = 0; int possible_hits = 0; @@ -562,9 +739,10 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; + tf_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + tf_msg.header.frame_id = tf_info.map_frame; + tf_msg.child_frame_id = tf_info.odom_frame; tf_msg.transform = tf2::toMsg(map2bf); RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); @@ -574,10 +752,11 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf) void AMCLLocalizer::publishParticles() { - geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseArray array_msg; + array_msg.header.stamp = last_input_time_; + array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { geometry_msgs::msg::Pose pose_msg; @@ -635,9 +814,10 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); double yaw_variance = computeYawVariance(particles_, 0, N_top); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.stamp = last_input_time_; + msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -659,9 +839,10 @@ AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; + odom_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_msg.header.frame_id = tf_info.map_frame; + odom_msg.child_frame_id = tf_info.robot_frame; tf2::Transform est_pose = getEstimatedPose(); diff --git a/localizers/easynav_fusion_localizer/CHANGELOG.rst b/localizers/easynav_fusion_localizer/CHANGELOG.rst new file mode 100644 index 00000000..60cdb0e5 --- /dev/null +++ b/localizers/easynav_fusion_localizer/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_fusion_localizer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2025-11-23) +------------------ +* First version +* Contributors: Miguel Ángel de Miguel diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt new file mode 100644 index 00000000..4db3bed8 --- /dev/null +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_fusion_localizer) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_localizer REQUIRED) +find_package(pluginlib REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(robot_localization REQUIRED) +find_package(std_srvs REQUIRED) +find_package(angles REQUIRED) + +# Geographiclib installs FindGeographicLib.cmake to this non-standard location +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) + +include_directories(${GeographicLib_INCLUDE_DIRS}) + + +set(dependencies + rclcpp + rclcpp_lifecycle + easynav_core + easynav_common + easynav_localizer + pluginlib + geographic_msgs + robot_localization +) + +add_library(fusion_localizer SHARED + src/easynav_fusion_localizer/FusionLocalizer.cpp + src/easynav_fusion_localizer/ukf_wrapper.cpp +) +target_include_directories(fusion_localizer PUBLIC + $ + $ + ${robot_localization_INCLUDE_DIRS} + ${std_srvs_INCLUDE_DIRS} +) +target_link_libraries(fusion_localizer + easynav_core::easynav_core + easynav_common::easynav_common + easynav_localizer::easynav_localizer + pluginlib::pluginlib + ${geographic_msgs_TARGETS} + GeographicLib + ${GeographicLib_LIBRARIES} + ${robot_localization_LIBRARIES} + angles::angles + yaets::yaets +) + +target_compile_definitions(fusion_localizer PUBLIC EASYNAV_DEBUG_WITH_YAETS) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(TARGETS + fusion_localizer + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries( + fusion_localizer +) +ament_export_targets( + export_${PROJECT_NAME} +) +# Register the localization plugins +pluginlib_export_plugin_description_file(easynav_core easynav_fusion_localizer_plugins.xml) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md new file mode 100644 index 00000000..16ef81a8 --- /dev/null +++ b/localizers/easynav_fusion_localizer/README.md @@ -0,0 +1,54 @@ +# easynav_gps_localizer + +[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) + +## Description +Odometry fusion localizer based on Robot Localization that fuses any n odometry sources. + +## Authors and Maintainers +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Miguel Ángel de Miguel + +## Supported ROS 2 Distributions +| Distribution | Status | +|---|---| +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | + +## Plugin (pluginlib) +- **Plugin Name:** `easynav_fusion_localizer/FusionLocalizer` +- **Type:** `easynav::FusionLocalizer` +- **Base Class:** `easynav::LocalizerMethodBase` +- **Library:** `fusion_localizer` +- **Description:** Odometry fusion localizer based on Robot Localization that fuses any n odometry sources. + +## Parameters + +TODO + +See [robot_localization documentation](https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html) + +## Interfaces (Topics and Services) + +### Subscriptions and Publications +| Direction | Topic | Type | Purpose | QoS | +|---|---|---|---|---| +| Publisher | `odometry/filtered` | `nav_msgs/msg/Odometry` | Odometry fused from all sources. | SensorDataQoS | + + +### Services +This package does not create service servers or clients. + +## NavState Keys +| Key | Type | Access | Notes | +|---|---|---|---| +| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | GPS-based odometry estimate. | + + +## TF Frames +| Role | Transform | Notes | +|---|---|---| +| Publishes | `map -> odom` | Static or slowly varying transform aligning UTM to local odometry frame. | + + +## License +GPL-3.0-only diff --git a/localizers/easynav_fusion_localizer/config/example_params.yaml b/localizers/easynav_fusion_localizer/config/example_params.yaml new file mode 100644 index 00000000..9c47f30b --- /dev/null +++ b/localizers/easynav_fusion_localizer/config/example_params.yaml @@ -0,0 +1,133 @@ +controller_node: + ros__parameters: + use_sim_time: true + controller_types: [dummy] + dummy: + rt_freq: 30.0 + plugin: easynav_controller/DummyController + cycle_time_nort: 0.01 + cycle_time_rt: 0.0001 + +localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [Ukf] + dummy: + rt_freq: 30.0 + plugin: easynav_localizer/DummyLocalizer + cycle_time_nort: 0.01 + cycle_time_rt: 0.001 + Ukf: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 0.1 + plugin: easynav_fusion_localizer/FusionLocalizer + latitude_origin: 40.2834931 + longitude_origin: -3.8207253999999997 + altitude_origin: 746.184 + local_filter: + two_d_mode: true + publish_tf: true + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # --- INPUT 1: GPS Odometry --- + gps0: /gps/fix + gps0_config: + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + gps0_differential: false + + #--- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + true, true, false, + false, false, false, + true, true, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + twist0: /gps/fix_velocity + twist0_config: + [false, false, false, + false, false, false, + true, false, false, + false, false, false, + false, false, false] + twist0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [dummy] + dummy: + freq: 10.0 + plugin: easynav_maps_manager/DummyMapsManager + cycle_time_nort: 0.1 + cycle_time_rt: 0.0001 + +planner_node: + ros__parameters: + use_sim_time: true + planner_types: [dummy] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + +sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [imu, gps] + perception_default_frame: odom + imu: + topic: /imu/data + type: sensor_msgs/msg/Imu + gps: + topic: /gps/fix + type: sensor_msgs/msg/NavSatFix + +system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.1 + angle_tolerance: 0.05 diff --git a/localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml b/localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml new file mode 100644 index 00000000..cf052497 --- /dev/null +++ b/localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml @@ -0,0 +1,9 @@ + + + + + A Kalman Filter implementation for the Localizer Method. + + + + diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp new file mode 100644 index 00000000..c29ce6e9 --- /dev/null +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include + +#include "easynav_core/LocalizerMethodBase.hpp" +#include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado + +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + +namespace easynav +{ + +/** + * @class FusionLocalizer + * @brief Plugin de localización para EasyNav que integra el UKF de + * robot_localization para la fusión de sensores. + */ +class FusionLocalizer : public easynav::LocalizerMethodBase +{ +public: + FusionLocalizer() = default; + virtual ~FusionLocalizer() = default; + +protected: + /** + * @brief Hook de inicialización de MethodBase. + * + * Loads and initializes the UkfWrapper, which will load parameters + * and create all robot_localization subscribers/publishers. + * + * @throws std::runtime_error if initialization fails. + */ + void on_initialize() override; + + /** + * @brief Hook de actualización RT (alta frecuencia) de LocalizerMethodBase. + * + * Esta función actúa como nuestro "timer" manual. Llama al ciclo + * principal (periodicUpdate) del filtro UKF y actualiza el NavState + * de easynav con la pose filtrada. + */ + void update_rt(NavState & nav_state) override; + + /** + * @brief Hook de actualización no-RT (baja frecuencia). + * + * En esta implementación, toda la lógica principal reside en update_rt. + */ + void update(NavState & nav_state) override; + +private: + std::unique_ptr ukf_wrapper_; + + int n_imu_sensors_{0}; + int n_gps_sensors_{0}; + + geometry_msgs::msg::PoseWithCovarianceStamped + navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg); + + double latitude_origin_{0.0}; + double longitude_origin_{0.0}; + double altitude_origin_{0.0}; + double UTM_origin_x_{0.0}; + double UTM_origin_y_{0.0}; + double UTM_origin_z_{0.0}; + std::string UTM_zone_; + + std::vector last_gps_stamp_; + +}; + +} // namespace easynav diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp new file mode 100644 index 00000000..d9dbde9c --- /dev/null +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -0,0 +1,878 @@ +/* + * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef EASYNAV_FUSION_LOCALIZER__UKF_WRAPPER_HPP_ +#define EASYNAV_FUSION_LOCALIZER__UKF_WRAPPER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "easynav_localizer/LocalizerNode.hpp" +#include "Eigen/Dense" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "robot_localization/filter_state.hpp" +#include "robot_localization/measurement.hpp" +#include "robot_localization/srv/toggle_filter_processing.hpp" +#include "robot_localization/srv/set_pose.hpp" +#include "robot_localization/ukf.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include +#include +#include + +namespace robot_localization +{ + +struct CallbackData +{ + CallbackData( + const std::string & topic_name, + const std::vector & update_vector, const int update_sum, + const bool differential, const bool relative, + const bool pose_use_child_frame, + const double rejection_threshold) + : topic_name_(topic_name), update_vector_(update_vector), + update_sum_(update_sum), differential_(differential), + relative_(relative), pose_use_child_frame_(pose_use_child_frame), + rejection_threshold_(rejection_threshold) {} + + std::string topic_name_; + std::vector update_vector_; + int update_sum_; + bool differential_; + bool relative_; + bool pose_use_child_frame_; + double rejection_threshold_; +}; + +using MeasurementQueue = + std::priority_queue, + Measurement>; +using MeasurementHistoryDeque = std::deque; +using FilterStateHistoryDeque = std::deque; + +class UkfWrapper +{ +public: + //! @brief Constructor + //! + //! The UkfWrapper constructor makes sure that anyone using + //! this template is doing so with the correct object type + //! + explicit UkfWrapper( + std::shared_ptr parent_node, + const std::string & tf_prefix, + const std::string & plugin_name); + + //! @brief Destructor + //! + //! Clears out the message filters and topic subscribers. + //! + ~UkfWrapper(); + + //! @brief Resets the filter to its initial state + //! + void reset(); + + //! @brief Service callback to toggle processing measurements for a standby + //! mode but continuing to publish + //! @param[in] request - The state requested, on (True) or off (False) + //! @param[out] response - status if upon success + //! @return boolean true if successful, false if not + //! + void toggleFilterProcessingCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Request> req, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Response> resp); + + //! @brief Callback method for receiving all acceleration (IMU) messages + //! @param[in] msg - The ROS IMU message to take in. + //! @param[in] callback_data - Relevant static callback data + //! @param[in] target_frame - The target frame_id into which to transform the + //! data + //! + void accelerationCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const CallbackData & callback_data, + const std::string & target_frame); + + //! @brief Callback method for receiving non-stamped control input + //! @param[in] msg - The ROS twist message to take in + //! + void controlCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + //! @brief Callback method for receiving stamped control input + //! @param[in] msg - The ROS stamped twist message to take in + //! + void + controlStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg); + + //! @brief Differentiate angular velocity for angular acceleration + //! + //! @param[in] currentTime - The time at which to carry out differentiation (the current time) + //! + //! Maybe more state variables can be time-differentiated to estimate higher-order states, + //! but now we only focus on obtaining the angular acceleration. It implements a backward- + //! Euler differentiation. + //! + void differentiateMeasurements(const rclcpp::Time & current_time); + + //! @brief Adds a measurement to the queue of measurements to be processed + //! + //! @param[in] topic_name - The name of the measurement source (only used for + //! debugging) + //! @param[in] measurement - The measurement to enqueue + //! @param[in] measurement_covariance - The covariance of the measurement + //! @param[in] update_vector - The boolean vector that specifies which + //! variables to update from this measurement + //! @param[in] mahalanobis_thresh - Threshold, expressed as a Mahalanobis + //! distance, for outlier rejection + //! @param[in] time - The time of arrival (in seconds) + //! + void enqueueMeasurement( + const std::string & topic_name, + const Eigen::VectorXd & measurement, + const Eigen::MatrixXd & measurement_covariance, + const std::vector & update_vector, + const double mahalanobis_thresh, + const rclcpp::Time & time); + + //! @brief Method for zeroing out 3D variables within measurements + //! @param[out] measurement - The measurement whose 3D variables will be + //! zeroed out + //! @param[out] measurement_covariance - The covariance of the measurement + //! @param[out] updateVector - The boolean update vector of the measurement + //! + //! If we're in 2D mode, then for every measurement from every sensor, we call + //! this. It sets the 3D variables to 0, gives those variables tiny variances, + //! and sets their updateVector values to 1. + //! + void forceTwoD( + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance, + std::vector & update_vector); + + //! @brief Method to get filter + //! @param[out] filter - the underlying templated filter + //! + robot_localization::Ukf & getFilter() + { + return filter_; + } + + //! @brief Retrieves the EKF's output for broadcasting + //! @param[out] message - The standard ROS odometry message to be filled + //! @return true if the filter is initialized, false otherwise + //! + bool getFilteredOdometryMessage(nav_msgs::msg::Odometry * message); + + //! @brief Retrieves the EKF's acceleration output for broadcasting + //! @param[out] message - The standard ROS acceleration message to be filled + //! @return true if the filter is initialized, false otherwise + //! + bool getFilteredAccelMessage( + geometry_msgs::msg::AccelWithCovarianceStamped * message); + + //! @brief Callback method for receiving all IMU messages + //! @param[in] msg - The ROS IMU message to take in. + //! @param[in] topic_name - The topic name for the IMU message (only used for + //! debug output) + //! @param[in] pose_callback_data - Relevant static callback data for + //! orientation variables + //! @param[in] twist_callback_data - Relevant static callback data for angular + //! velocity variables + //! @param[in] accel_callback_data - Relevant static callback data for linear + //! acceleration variables + //! + //! This method separates out the orientation, angular velocity, and linear + //! acceleration data and passed each on to its respective callback. + //! + void imuCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data, + const CallbackData & accel_callback_data); + + //! @brief Processes all measurements in the measurement queue, in temporal + //! order + //! + //! @param[in] current_time - The time at which to carry out integration (the + //! current time) + //! + void integrateMeasurements(const rclcpp::Time & current_time); + + //! @brief Loads all parameters from file + //! + void loadParams(); + + //! @brief callback function which is called for periodic updates + //! + void periodicUpdate(); + + //! @brief Callback method for receiving all odometry messages + //! @param[in] msg - The ROS odometry message to take in. + //! @param[in] topic_name - The topic name for the odometry message (only used + //! for debug output) + //! @param[in] pose_callback_data - Relevant static callback data for pose + //! variables + //! @param[in] twist_callback_data - Relevant static callback data for twist + //! variables + //! + //! This method simply separates out the pose and twist data into two new + //! messages, and passes them into their respective callbacks + //! + void odometryCallback( + const nav_msgs::msg::Odometry::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data); + + //! @brief Callback method for receiving all pose messages + //! @param[in] msg - The ROS stamped pose with covariance message to take in + //! @param[in] callback_data - Relevant static callback data + //! @param[in] target_frame - The target frame_id into which to transform the + //! data + //! @param[in] pose_source_frame - The source frame_id from which to transform + //! the data + //! @param[in] imu_data - Whether this data comes from an IMU + //! + void poseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame, + const std::string & pose_source_frame, + const bool imu_data); + + //! @brief initialize the filter + //! + void initialize(); + + //! @brief Service callback for resetting the filter to its initial state. Parameters are unused. + //! + void resetSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr); + + //! @brief Callback method for manually setting/resetting the internal pose + //! estimate + //! @param[in] msg - The ROS stamped pose with covariance message to take in + //! + void setPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + //! @brief Service callback for manually setting/resetting the internal pose + //! estimate + //! + //! @param[in] request - Custom service request with pose information + //! @return true if successful, false if not + bool setPoseSrvCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + //! @brief Service callback for manually enable the filter + //! @param[in] request - N/A + //! @param[out] response - N/A + //! @return boolean true if successful, false if not + bool enableFilterSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr); + + //! @brief Callback method for receiving all twist messages + //! @param[in] msg - The ROS stamped twist with covariance message to take in. + //! @param[in] callback_data - Relevant static callback data + //! @param[in] target_frame - The target frame_id into which to transform the + //! data + //! + void twistCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame); + + //! @brief Validates filter outputs for NaNs and Infinite values + //! @param[out] message - The standard ROS odometry message to be validated + //! @return true if the filter output is valid, false otherwise + //! + bool validateFilterOutput(nav_msgs::msg::Odometry * message); + + std::vector getGpsCallbackDataArr() const + { + return gps_callbackData_arr_; + } + +protected: + //! @brief Finds the latest filter state before the given timestamp and makes + //! it the current state again. + //! + //! This method also inserts all measurements between the older filter + //! timestamp and now into the measurements queue. + //! + //! @param[in] time - The time to which the filter state should revert + //! @return True if restoring the filter succeeded. False if not. + //! + bool revertTo(const rclcpp::Time & time); + + //! @brief Saves the current filter state in the queue of previous filter + //! states + //! + //! These measurements will be used in backwards smoothing in the event that + //! older measurements come in. + //! @param[in] filter - The filter base object whose state we want to save + //! + void saveFilterState(robot_localization::Ukf & filter); + + //! @brief Removes measurements and filter states older than the given cutoff + //! time. + //! @param[in] cutoff_time - Measurements and states older than this time will + //! be dropped. + //! + void clearExpiredHistory(const rclcpp::Time cutoff_time); + + //! @brief Clears measurement queue + //! + void clearMeasurementQueue(); + + //! @brief Adds a diagnostic message to the accumulating map and updates the + //! error level + //! @param[in] error_level - The error level of the diagnostic + //! @param[in] topic_and_class - The sensor topic (if relevant) and class of + //! diagnostic + //! @param[in] message - Details of the diagnostic + //! @param[in] is_static - Whether or not this diagnostic information is + //! static + //! + void addDiagnostic( + const int error_level, const std::string & topic_and_class, + const std::string & message, const bool is_static); + + //! @brief Aggregates all diagnostics so they can be published + //! @param[in] wrapper - The diagnostic status wrapper to update + //! + void aggregateDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper & wrapper); + + //! @brief Utility method for copying covariances from ROS covariance arrays + //! to Eigen matrices + //! + //! This method copies the covariances and also does some data validation, + //! which is why it requires more parameters than just the covariance + //! containers. + //! @param[in] covariance_in - The source array for the covariance data + //! @param[in] covariance_out - The destination matrix for the covariance data + //! @param[in] topic_name - The name of the source data topic (for debug + //! purposes) + //! @param[in] update_vector - The update vector for the source topic + //! @param[in] offset - The "starting" location within the array/update vector + //! @param[in] dimension - The number of values to copy, starting at the + //! offset + //! + void copyCovariance( + const double * covariance_in, + Eigen::MatrixXd & covariance_out, + const std::string & topic_name, + const std::vector & update_vector, + const size_t offset, const size_t dimension); + + //! @brief Utility method for copying covariances from Eigen matrices to ROS + //! covariance arrays + //! + //! @param[in] covariance_in - The source matrix for the covariance data + //! @param[in] covariance_out - The destination array + //! @param[in] dimension - The number of values to copy + //! + void copyCovariance( + const Eigen::MatrixXd & covariance_in, + double * covariance_out, const size_t dimension); + + //! @brief Loads fusion settings from the config file + //! @param[in] topic_name - The name of the topic for which to load settings + //! @return The boolean vector of update settings for each variable for this + //! topic + //! + std::vector loadUpdateConfig(const std::string & topic_name); + + //! @brief Prepares an IMU message's linear acceleration for integration into + //! the filter + //! @param[in] msg - The IMU message to prepare + //! @param[in] topic_name - The name of the topic over which this message was + //! received + //! @param[in] target_frame - The target tf frame + //! @param[in] relative - whether the IMU sensor reports pose relative to + //! initialization pose + //! @param[in] update_vector - The update vector for the data source + //! @param[in] measurement - The twist data converted to a measurement + //! @param[in] measurement_covariance - The covariance of the converted + //! measurement + //! + bool prepareAcceleration( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const std::string & target_frame, + const bool relative, + std::vector & update_vector, + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance); + + //! @brief Prepares a pose message for integration into the filter + //! @param[in] msg - The pose message to prepare + //! @param[in] topic_name - The name of the topic over which this message was + //! received + //! @param[in] target_frame - The target tf frame + //! @param[in] source_frame - The source tf frame + //! @param[in] differential - Whether we're carrying out differential + //! integration + //! @param[in] relative - Whether this measurement is processed relative to + //! the first + //! @param[in] imuData - Whether this measurement is from an IMU + //! @param[in,out] updateVector - The update vector for the data source + //! @param[out] measurement - The pose data converted to a measurement + //! @param[out] measurementCovariance - The covariance of the converted + //! measurement + //! @return true indicates that the measurement was successfully prepared, + //! false otherwise + //! + bool preparePose( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const std::string & topic_name, const std::string & target_frame, + const std::string & source_frame, + const bool differential, const bool relative, const bool imu_data, + std::vector & update_vector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance); + + //! @brief Prepares a twist message for integration into the filter + //! @param[in] msg - The twist message to prepare + //! @param[in] topicName - The name of the topic over which this message was + //! received + //! @param[in] targetFrame - The target tf frame + //! @param[in,out] updateVector - The update vector for the data source + //! @param[out] measurement - The twist data converted to a measurement + //! @param[out] measurementCovariance - The covariance of the converted + //! measurement + //! @return true indicates that the measurement was successfully prepared, + //! false otherwise + //! + bool prepareTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const std::string & topicName, const std::string & targetFrame, + std::vector & updateVector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurementCovariance); + + std::shared_ptr parent_node_{nullptr}; + + //! @brief Whether or not we print diagnostic messages to the /diagnostics + //! topic + //! + bool print_diagnostics_; + + //! @brief Whether we publish the acceleration + //! + bool publish_acceleration_; + + //! @brief Whether we publish the transform from the world_frame to the + //! base_link_frame + //! + bool publish_transform_; + + //! @brief Whether to reset the filters when backwards jump in time is + //! detected + //! + //! This is usually the case when logs are being used and a jump in the logi + //! is done or if a log files restarts from the beginning. + //! + bool reset_on_time_jump_; + + //! @brief Whether or not we use smoothing + //! + bool smooth_lagged_data_; + + //! @brief Whether the filter should process new measurements or not. + //! + bool toggled_on_; + + //! @brief Whether or not we're in 2D mode + //! + //! If this is true, the filter binds all 3D variables (Z, + //! roll, pitch, and their respective velocities) to 0 for + //! every measurement. + //! + bool two_d_mode_; + + //! @brief Whether or not we use a control term + //! + bool use_control_; + + //! @brief Whether or not we use a stamped control term + //! + bool stamped_control_; + + //! @brief Start the Filter disabled at startup + //! + //! If this is true, the filter reads parameters and prepares publishers and subscribes + //! but does not integrate new messages into the state vector. + //! The filter can be enabled later using a service. + bool disabled_at_startup_; + + //! @brief Whether the filter is enabled or not. See disabledAtStartup_. + bool enabled_; + + //! @brief Whether we'll allow old measurements to cause a re-publication of the updated state + bool permit_corrected_publication_; + + //! @brief The max (worst) dynamic diagnostic level. + //! + int dynamic_diag_error_level_; + + //! @brief The max (worst) static diagnostic level. + //! + int static_diag_error_level_; + + //! @brief The frequency of the run loop + //! + double frequency_; + + //! @brief What is the acceleration in Z due to gravity (m/s^2)? Default is + //! +9.80665. + //! + double gravitational_acceleration_; + + //! @brief The depth of the history we track for smoothing/delayed measurement + //! processing + //! + //! This is the guaranteed minimum buffer size for which previous states and + //! measurements are kept. + //! + rclcpp::Duration history_length_; + + //! @brief The sensor timeout value that gets passed to the core filter + //! + rclcpp::Duration sensor_timeout_; + + //! @brief tf frame name for the robot's body frame + //! + std::string base_link_frame_id_; + + //! @brief tf frame name for the robot's body frame + //! + //! When the final state is computed, we "override" the output transform and + //! message to have this frame for its child_frame_id. This helps to enable + //! disconnected TF trees when multiple EKF instances are being run. + //! + std::string base_link_output_frame_id_; + + //! @brief tf frame name for the robot's map (world-fixed) frame + //! + std::string map_frame_id_; + + //! @brief tf frame name for the robot's odometry (world-fixed) frame + //! + std::string odom_frame_id_; + + //! @brief tf frame name that is the parent frame of the transform that this + //! node will calculate and broadcast. + //! + std::string world_frame_id_; + + //! @brief Used for outputting debug messages + //! + std::ofstream debug_stream_; + + //! @brief The most recent control input + //! + Eigen::VectorXd latest_control_; + + //! @brief The process noise covariance matrix that gets passed to the core filter + //! + Eigen::MatrixXd process_noise_covariance_; + + //! @brief The initial estimate error covariance matrix that gets passed to the core filter + //! + Eigen::MatrixXd initial_estimate_error_covariance_; + + //! @brief Message that contains our latest transform (i.e., state) + //! + //! We use the vehicle's latest state in a number of places, and often + //! use it as a transform, so this is the most convenient variable to + //! use as our global "current state" object + //! + geometry_msgs::msg::TransformStamped world_base_link_trans_msg_; + + //! @brief last call of periodicUpdate + //! + rclcpp::Time last_diag_time_; + + //! @brief The time of the most recent published state + //! + rclcpp::Time last_published_stamp_; + + //! @brief We process measurements by queueing them up in + //! callbacks and processing them all at once within each + //! iteration + //! + MeasurementQueue measurement_queue_; + + //! @brief Contains the state vector variable names in string format + //! + std::vector state_variable_names_; + + //! @brief This object accumulates dynamic diagnostics, e.g., diagnostics + //! relating to sensor data. + //! + //! The values are considered transient and are cleared at every iteration. + //! + std::map dynamic_diagnostics_; + + //! @brief This object accumulates static diagnostics, e.g., diagnostics + //! relating to the configuration parameters. + //! + //! The values are treated as static and always reported (i.e., this object is + //! never cleared) + //! + std::map static_diagnostics_; + + //! @brief Store the last time a message from each topic was received + //! + //! If we're getting messages rapidly, we may accidentally get + //! an older message arriving after a newer one. This variable keeps + //! track of the most recent message time for each subscribed message + //! topic. We also use it when listening to odometry messages to + //! determine if we should be using messages from that topic. + //! + std::map last_message_times_; + + //! @brief Last time mark that time-differentiation is calculated, in seconds + //! + double last_diff_time_; + + //! @brief Last record of filtered angular velocity + //! + tf2::Vector3 last_state_twist_rot_; + + //! @brief Calculated angular acceleration from time-differencing + //! + tf2::Vector3 angular_acceleration_; + + //! @brief Covariance of the calculated angular acceleration + //! + Eigen::Matrix3d angular_acceleration_cov_; + + //! @brief Stores the first measurement from each topic for relative + //! measurements + //! + //! When a given sensor is being integrated in relative mode, its first + //! measurement is effectively treated as an offset, and future measurements + //! have this first measurement removed before they are fused. This variable + //! stores the initial measurements. Note that this is different from using + //! differential mode, as in differential mode, pose data is converted to + //! twist data, resulting in boundless error growth for the variables being + //! fused. With relative measurements, the vehicle will start with a 0 heading + //! and position, but the measurements are still fused absolutely. + std::map initial_measurements_; + + //! @brief If including acceleration for each IMU input, whether or not we + //! remove acceleration due to gravity + //! + std::map remove_gravitational_acceleration_; + + //! @brief An implicitly time ordered queue of past filter states used for + //! smoothing. + // + // front() refers to the filter state with the earliest timestamp. + // back() refers to the filter state with the latest timestamp. + FilterStateHistoryDeque filter_state_history_; + + //! @brief A deque of previous measurements which is implicitly ordered from + //! earliest to latest measurement. + // when popped from the measurement priority queue. + // front() refers to the measurement with the earliest timestamp. + // back() refers to the measurement with the latest timestamp. + MeasurementHistoryDeque measurement_history_; + + //! @brief Vector to hold our subscribers until they go out of scope + //! + std::vector topic_subs_; + + //! @brief Stores the last measurement from a given topic for differential + //! integration + //! + //! To carry out differential integration, we have to (1) transform + //! that into the target frame (probably the frame specified by + //! @p odomFrameId_), (2) "subtract" the previous measurement from + //! the current measurement, and then (3) transform it again by the previous + //! state estimate. This holds the measurements used for step (2). + //! + std::map previous_measurements_; + + //! @brief We also need the previous covariance matrix for differential data + //! + std::map previous_measurement_covariances_; + + //! @brief By default, the filter predicts and corrects up to the time of the + //! latest measurement. If this is set to true, the filter does the same, but + //! then also predicts up to the current time step. + //! + bool predict_to_current_time_; + + //! @brief Store the last time set pose message was received + //! + //! If we receive a setPose message to reset the filter, we can get in + //! strange situations wherein we process the pose reset, but then even + //! after another spin cycle or two, we can get a new message with a time + //! stamp that is *older* than the setPose message's time stamp. This means + //! we have to check the message's time stamp against the lastSetPoseTime_. + rclcpp::Time last_set_pose_time_; + + //! @brief The time of the most recent control input + //! + rclcpp::Time latest_control_time_; + + //! @brief Parameter that specifies the how long we wait for a transform to + //! become available. + //! + rclcpp::Duration tf_timeout_; + + //! @brief For future (or past) dating the world_frame->base_link_frame + //! transform + //! + rclcpp::Duration tf_time_offset_; + + //! @brief Service that allows another node to toggle on/off filter + //! processing while still publishing. + //! Uses a robot_localization ToggleFilterProcessing service. + //! + rclcpp::Service::SharedPtr + toggle_filter_processing_srv_; + + //! @brief Subscribes to the control input topic + //! + rclcpp::Subscription::SharedPtr control_sub_; + + //! @brief Subscribes to the control stamped input topic + //! + rclcpp::Subscription::SharedPtr stamped_control_sub_; + + //! @brief Subscribes to the set_pose topic (usually published from rviz). + //! Message type is geometry_msgs/PoseWithCovarianceStamped. + //! + rclcpp::Subscription::SharedPtr + set_pose_sub_; + + //! @brief Service that allows another node to change the current state and + //! recieve a confirmation. Uses a custom SetPose service. + //! + rclcpp::Service::SharedPtr + set_pose_service_; + + //! @brief Service that allows another node to enable the filter. Uses a + //! standard Empty service. + //! + rclcpp::Service::SharedPtr enable_filter_srv_; + + //! @brief Service that resets the filter to its initial state + //! + rclcpp::Service::SharedPtr reset_srv_; + + //! @brief Transform buffer for managing coordinate transforms + //! + std::unique_ptr tf_buffer_; + + //! @brief Transform listener for receiving transforms + //! + std::unique_ptr tf_listener_; + + //! @brief broadcaster of worldTransform tfs + //! + std::shared_ptr world_transform_broadcaster_; + + //! @brief Used for updating the diagnostics + //! + std::unique_ptr diagnostic_updater_; + + //! @brief Position publisher + //! + rclcpp::Publisher::SharedPtr position_pub_; + + //! Acceleration publisher + //! + rclcpp::Publisher::SharedPtr + accel_pub_; + + //! @brief Our filter (EKF, UKF, etc.) + //! + robot_localization::Ukf filter_; + + //! @brief Timer for filter updates + //! + rclcpp::TimerBase::SharedPtr timer_; + + //! @brief optional signaling diagnostic frequency + //! + std::unique_ptr freq_diag_; + + //! @brief minimum frequency threshold for frequency diagnostic + //! Must be on heap since pointer is passed to diagnostic_updater::FrequencyStatusParam + //! + double min_frequency_; + + //! @brief maximum frequency threshold for frequency diagnostic + //! Must be on heap since pointer is passed to diagnostic_updater::FrequencyStatusParam + //! + double max_frequency_; + + std::string plugin_name_; + std::string tf_prefix_; + std::vector gps_callbackData_arr_; + +}; + +} // namespace robot_localization + +#endif // ROBOT_LOCALIZATION__ROS_FILTER_HPP_ diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml new file mode 100644 index 00000000..5f08b86a --- /dev/null +++ b/localizers/easynav_fusion_localizer/package.xml @@ -0,0 +1,32 @@ + + + + easynav_fusion_localizer + 0.0.2 + Easy Navigation: Fusion Localizer package. + Miguel Ángel de Miguel + + GPL-3.0-only + + ament_cmake + + rclcpp + rclcpp_lifecycle + easynav_core + easynav_common + easynav_localizer + pluginlib + geographiclib + geographic_msgs + robot_localization + std_srvs + angles + + ament_lint_auto + ament_lint_common + robot_localization + + + ament_cmake + + diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp new file mode 100644 index 00000000..b212b73f --- /dev/null +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -0,0 +1,168 @@ +#include "easynav_fusion_localizer/FusionLocalizer.hpp" + +#include "easynav_localizer/LocalizerNode.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_common/types/IMUPerception.hpp" +#include "easynav_common/types/GNSSPerception.hpp" + +#include + +#include "easynav_common/YTSession.hpp" + +namespace easynav +{ + +void FusionLocalizer::on_initialize() +{ + + try { + + auto node = get_node(); + + auto localizer_node = std::dynamic_pointer_cast(node); + + last_gps_stamp_.resize(10, 0.0); + + const std::string & plugin_name = this->get_plugin_name(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + RCLCPP_INFO(localizer_node->get_logger(), "Using tf_prefix: '%s'", tf_info.tf_prefix.c_str()); + RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'", + plugin_name.c_str()); + + ukf_wrapper_ = std::make_unique( + localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter" + ); + ukf_wrapper_->initialize(); + localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); + + localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_); + + localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); + localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); + } catch (const std::exception & e) { + RCLCPP_FATAL( + get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", + e.what()); + // Raise error + throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what()); + } + + + int zone; + bool northp; + + GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, zone, northp, UTM_origin_x_, + UTM_origin_y_); + UTM_zone_ = std::to_string(zone) + (northp ? "N" : "S"); + UTM_origin_z_ = altitude_origin_; + + n_gps_sensors_ = static_cast(ukf_wrapper_->getGpsCallbackDataArr().size()); + + RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); +} + +// 2. Hook de actualización RT (Tu "Timer" de alta frecuencia) +void FusionLocalizer::update_rt(NavState & nav_state) +{ + if(n_gps_sensors_ && nav_state.has("gnss")) { + auto gps_data = nav_state.get(std::string("gnss")); + for (int i = 0; i < n_gps_sensors_; ++i) { + double gps_time = gps_data[i]->data.header.stamp.sec + + gps_data[i]->data.header.stamp.nanosec * 1e-9; + if (gps_time > last_gps_stamp_[i]) { + // if(true) { + last_gps_stamp_[i] = gps_time; + auto pose = navsatfix_to_pose(gps_data[i]->data); + // nav_state.set("UTM_gnss_pose", pose); + // Call the wrapper callback + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + ukf_wrapper_->poseCallback( + std::make_shared(pose), + ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data + tf_info.map_frame, // target_frame + tf_info.odom_frame, // pose_source_frame + false // imu_data + ); + } + } + } + + ukf_wrapper_->periodicUpdate(); + + nav_msgs::msg::Odometry current_odom; + if (ukf_wrapper_->getFilteredOdometryMessage(¤t_odom)) { + nav_state.set("robot_pose", current_odom); + } +} + +// 3. Hook de actualización no-RT (baja frecuencia) +void FusionLocalizer::update([[maybe_unused]] NavState & nav_state) +{ + +} + +geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose( + const sensor_msgs::msg::NavSatFix & navsat_msg) +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // 1. Establecer el Header + // Usamos el mismo timestamp que el mensaje original + // y el world_frame_id que el filtro UKF espera (p.ej., "map" u "odom") + pose_msg.header = navsat_msg.header; + + pose_msg.header.frame_id = tf_info.map_frame; + + // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) + double utm_x, utm_y; + int zone; + bool northp; + + GeographicLib::UTMUPS::Forward( + navsat_msg.latitude, + navsat_msg.longitude, + zone, + northp, + utm_x, + utm_y); + + pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_; + pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_; + pose_msg.pose.pose.position.z = navsat_msg.altitude - UTM_origin_z_; + + pose_msg.pose.pose.orientation.x = 0.0; + pose_msg.pose.pose.orientation.y = 0.0; + pose_msg.pose.pose.orientation.z = 0.0; + pose_msg.pose.pose.orientation.w = 1.0; + + pose_msg.pose.covariance.fill(0.0); + + pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0]; // xx + pose_msg.pose.covariance[1] = navsat_msg.position_covariance[1]; // xy + pose_msg.pose.covariance[2] = navsat_msg.position_covariance[2]; // xz + + pose_msg.pose.covariance[6] = navsat_msg.position_covariance[3]; // yx + pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4]; // yy + pose_msg.pose.covariance[8] = navsat_msg.position_covariance[5]; // yz + + pose_msg.pose.covariance[12] = navsat_msg.position_covariance[6]; // zx + pose_msg.pose.covariance[13] = navsat_msg.position_covariance[7]; // zy + pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; // zz + + pose_msg.pose.covariance[21] = 99999.0; + pose_msg.pose.covariance[28] = 99999.0; + pose_msg.pose.covariance[35] = 99999.0; + + return pose_msg; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::FusionLocalizer, easynav::LocalizerMethodBase) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp new file mode 100644 index 00000000..ec2f87da --- /dev/null +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -0,0 +1,3789 @@ +/* + * Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include "easynav_fusion_localizer/ukf_wrapper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_updater/diagnostic_status_wrapper.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "Eigen/Dense" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/rclcpp.hpp" +#include "robot_localization/ekf.hpp" +#include "robot_localization/filter_common.hpp" +#include "robot_localization/filter_state.hpp" +#include "robot_localization/filter_utilities.hpp" +#include "robot_localization/ros_filter_utilities.hpp" +#include "robot_localization/srv/set_pose.hpp" +#include "robot_localization/srv/toggle_filter_processing.hpp" +#include "robot_localization/ukf.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2/LinearMath/Vector3.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include +#include +#include + +#include "easynav_common/RTTFBuffer.hpp" + +namespace robot_localization +{ +using namespace std::chrono_literals; + +UkfWrapper::UkfWrapper( + std::shared_ptr parent_node, + const std::string & tf_prefix, + const std::string & plugin_name) +: print_diagnostics_(true), + publish_acceleration_(false), + publish_transform_(true), + reset_on_time_jump_(false), + smooth_lagged_data_(false), + toggled_on_(true), + two_d_mode_(false), + use_control_(false), + stamped_control_(true), + disabled_at_startup_(false), + enabled_(false), + permit_corrected_publication_(false), + dynamic_diag_error_level_(diagnostic_msgs::msg::DiagnosticStatus::OK), + static_diag_error_level_(diagnostic_msgs::msg::DiagnosticStatus::OK), + frequency_(30.0), + gravitational_acceleration_(9.80665), + history_length_(0ns), + sensor_timeout_(0ns), + latest_control_(), + process_noise_covariance_(STATE_SIZE, STATE_SIZE), + initial_estimate_error_covariance_(STATE_SIZE, STATE_SIZE), + last_diag_time_(0, 0, RCL_ROS_TIME), + last_published_stamp_(0, 0, RCL_ROS_TIME), + predict_to_current_time_(false), + last_set_pose_time_(0, 0, RCL_ROS_TIME), + latest_control_time_(0, 0, RCL_ROS_TIME), + tf_timeout_(0ns), + tf_time_offset_(0ns) +{ + parent_node_ = parent_node; + tf_prefix_ = tf_prefix; + plugin_name_ = plugin_name; + + tf_buffer_ = std::make_unique(parent_node_->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + + state_variable_names_.push_back("X"); + state_variable_names_.push_back("Y"); + state_variable_names_.push_back("Z"); + state_variable_names_.push_back("ROLL"); + state_variable_names_.push_back("PITCH"); + state_variable_names_.push_back("YAW"); + state_variable_names_.push_back("X_VELOCITY"); + state_variable_names_.push_back("Y_VELOCITY"); + state_variable_names_.push_back("Z_VELOCITY"); + state_variable_names_.push_back("ROLL_VELOCITY"); + state_variable_names_.push_back("PITCH_VELOCITY"); + state_variable_names_.push_back("YAW_VELOCITY"); + state_variable_names_.push_back("X_ACCELERATION"); + state_variable_names_.push_back("Y_ACCELERATION"); + state_variable_names_.push_back("Z_ACCELERATION"); +} + +UkfWrapper::~UkfWrapper() +{ + topic_subs_.clear(); + timer_.reset(); + set_pose_sub_.reset(); + control_sub_.reset(); + stamped_control_sub_.reset(); + tf_listener_.reset(); + tf_buffer_.reset(); + diagnostic_updater_.reset(); + world_transform_broadcaster_.reset(); + set_pose_service_.reset(); + freq_diag_.reset(); + accel_pub_.reset(); + position_pub_.reset(); +} + +void UkfWrapper::reset() +{ + // Get rid of any initial poses (pretend we've never had a measurement) + initial_measurements_.clear(); + previous_measurements_.clear(); + previous_measurement_covariances_.clear(); + + clearMeasurementQueue(); + + filter_state_history_.clear(); + measurement_history_.clear(); + + angular_acceleration_.setZero(); + angular_acceleration_cov_.setIdentity(); + angular_acceleration_cov_ *= 0.01; + + last_state_twist_rot_.setZero(); + + // Also set the last set pose time, so we ignore all messages + // that occur before it + last_set_pose_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + last_diag_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + latest_control_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + last_published_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + last_diff_time_ = parent_node_->now().seconds(); + + // clear tf buffer to avoid TF_OLD_DATA errors + tf_buffer_->clear(); + + // clear last message timestamp, so older messages will be accepted + last_message_times_.clear(); + + // reset filter to uninitialized state + filter_.reset(); + + // Restore filter parameters that we got from the ROS parameter server + filter_.setSensorTimeout(sensor_timeout_); + filter_.setProcessNoiseCovariance(process_noise_covariance_); + filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); +} + +void UkfWrapper::resetSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr) +{ + RCLCPP_INFO( + parent_node_->get_logger(), + "Received a request to reset filter."); + + reset(); +} + +void UkfWrapper::toggleFilterProcessingCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Request> req, + const std::shared_ptr< + robot_localization::srv::ToggleFilterProcessing::Response> resp) +{ + if (req->on == toggled_on_) { + RCLCPP_WARN( + parent_node_->get_logger(), + "Service was called to toggle filter processing but state was already as " + "requested."); + resp->status = false; + } else { + RCLCPP_INFO( + parent_node_->get_logger(), + "Toggling filter measurement filtering to %s.", req->on ? "On" : "Off"); + toggled_on_ = req->on; + resp->status = true; + } +} + +// @todo: Replace with AccelWithCovarianceStamped +void UkfWrapper::accelerationCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const CallbackData & callback_data, + const std::string & target_frame) +{ + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + return; + } + + const std::string & topic_name = callback_data.topic_name_; + + RF_DEBUG( + "------ UkfWrapper::accelerationCallback (" << topic_name << + ") ------\n") + // "Twist message:\n" << *msg); + + if (last_message_times_.count(topic_name) == 0) { + last_message_times_.insert( + std::pair(topic_name, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (last_message_times_[topic_name] <= msg->header.stamp) { + RF_DEBUG("Update vector for " << topic_name << " is:\n" << topic_name); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurement_covariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector update_vector_corrected = callback_data.update_vector_; + + // Prepare the twist data for inclusion in the filter + if (prepareAcceleration( + msg, topic_name, target_frame, callback_data.relative_, + update_vector_corrected, measurement, + measurement_covariance)) + { + // Store the measurement. Add an "acceleration" suffix so we know what + // kind of measurement we're dealing with when we debug the core filter + // logic. + enqueueMeasurement( + topic_name, measurement, measurement_covariance, + update_vector_corrected, + callback_data.rejection_threshold_, msg->header.stamp); + + RF_DEBUG( + "Enqueued new measurement for " << topic_name << + "_acceleration\n"); + } else { + RF_DEBUG( + "Did *not* enqueue measurement for " << topic_name << + "_acceleration\n"); + } + + last_message_times_[topic_name] = msg->header.stamp; + + RF_DEBUG( + "Last message time for " << + topic_name << " is now " << + filter_utilities::toSec(last_message_times_[topic_name]) << + "\n"); + } else { + // else if (reset_on_time_jump_ && rclcpp::Time::isSimTime()) + //{ + // reset(); + //} + + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp before that of " + "the previous message received," << " this message will be ignored. This may" + " indicate a bad timestamp. (message time: " << msg->header.stamp.nanosec << + ")"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, topic_name + + "_timestamp", stream.str(), false); + + RF_DEBUG( + "Message is too old. Last message time for " << + topic_name << " is " << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << + filter_utilities::toSec(msg->header.stamp) << ".\n"); + } + + RF_DEBUG( + "\n----- /UkfWrapper::accelerationCallback (" << topic_name << + ") ------\n"); +} + +void UkfWrapper::controlCallback( + const geometry_msgs::msg::Twist::SharedPtr msg) +{ + geometry_msgs::msg::TwistStamped::SharedPtr twist_stamped_ptr = + std::make_shared(); + twist_stamped_ptr->twist = *msg; + twist_stamped_ptr->header.frame_id = base_link_frame_id_; + twist_stamped_ptr->header.stamp = parent_node_->now(); + controlStampedCallback(twist_stamped_ptr); +} + +void UkfWrapper::controlStampedCallback( + const geometry_msgs::msg::TwistStamped::SharedPtr msg) +{ + if (msg->header.frame_id == base_link_frame_id_ || + msg->header.frame_id == "") + { + latest_control_(ControlMemberVx) = msg->twist.linear.x; + latest_control_(ControlMemberVy) = msg->twist.linear.y; + latest_control_(ControlMemberVz) = msg->twist.linear.z; + latest_control_(ControlMemberVroll) = msg->twist.angular.x; + latest_control_(ControlMemberVpitch) = msg->twist.angular.y; + latest_control_(ControlMemberVyaw) = msg->twist.angular.z; + latest_control_time_ = msg->header.stamp; + + // Update the filter with this control term + filter_.setControl(latest_control_, msg->header.stamp); + } else { + RCLCPP_WARN_STREAM_THROTTLE( + parent_node_->get_logger(), *parent_node_->get_clock(), 5000, "Commanded velocities " + " must be given in the robot's body frame (" << base_link_frame_id_ << + "). Message frame was " << msg->header.frame_id); + } +} + +void UkfWrapper::enqueueMeasurement( + const std::string & topic_name, const Eigen::VectorXd & measurement, + const Eigen::MatrixXd & measurement_covariance, + const std::vector & update_vector, const double mahalanobis_thresh, + const rclcpp::Time & time) +{ + MeasurementPtr meas = MeasurementPtr(new Measurement()); + + meas->topic_name_ = topic_name; + meas->measurement_ = measurement; + meas->covariance_ = measurement_covariance; + meas->update_vector_ = update_vector; + meas->time_ = time; + meas->mahalanobis_thresh_ = mahalanobis_thresh; + meas->latest_control_ = latest_control_; + meas->latest_control_time_ = latest_control_time_; + measurement_queue_.push(meas); +} + +void UkfWrapper::forceTwoD( + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance, + std::vector & update_vector) +{ + // Force 3D variables to 0 in the measurement + measurement(StateMemberZ) = 0.0; + measurement(StateMemberRoll) = 0.0; + measurement(StateMemberPitch) = 0.0; + measurement(StateMemberVz) = 0.0; + measurement(StateMemberVroll) = 0.0; + measurement(StateMemberVpitch) = 0.0; + measurement(StateMemberAz) = 0.0; + + // Need to eliminate any off-diagonal covariance values that involve one of our 3D variables + measurement_covariance.col(StateMemberZ).fill(0.0); + measurement_covariance.col(StateMemberRoll).fill(0.0); + measurement_covariance.col(StateMemberPitch).fill(0.0); + measurement_covariance.col(StateMemberVz).fill(0.0); + measurement_covariance.col(StateMemberVroll).fill(0.0); + measurement_covariance.col(StateMemberVpitch).fill(0.0); + measurement_covariance.col(StateMemberAz).fill(0.0); + + measurement_covariance.row(StateMemberZ).fill(0.0); + measurement_covariance.row(StateMemberRoll).fill(0.0); + measurement_covariance.row(StateMemberPitch).fill(0.0); + measurement_covariance.row(StateMemberVz).fill(0.0); + measurement_covariance.row(StateMemberVroll).fill(0.0); + measurement_covariance.row(StateMemberVpitch).fill(0.0); + measurement_covariance.row(StateMemberAz).fill(0.0); + + // Now set the diagonal covariance values to something small + measurement_covariance(StateMemberZ, StateMemberZ) = 1e-6; + measurement_covariance(StateMemberRoll, StateMemberRoll) = 1e-6; + measurement_covariance(StateMemberPitch, StateMemberPitch) = 1e-6; + measurement_covariance(StateMemberVz, StateMemberVz) = 1e-6; + measurement_covariance(StateMemberVroll, StateMemberVroll) = 1e-6; + measurement_covariance(StateMemberVpitch, StateMemberVpitch) = 1e-6; + measurement_covariance(StateMemberAz, StateMemberAz) = 1e-6; + + // Finally, update the update vector + update_vector[StateMemberZ] = 1; + update_vector[StateMemberRoll] = 1; + update_vector[StateMemberPitch] = 1; + update_vector[StateMemberVz] = 1; + update_vector[StateMemberVroll] = 1; + update_vector[StateMemberVpitch] = 1; + update_vector[StateMemberAz] = 1; +} + +bool UkfWrapper::getFilteredOdometryMessage(nav_msgs::msg::Odometry * message) +{ + // If the filter has received a measurement at some point... + if (filter_.getInitializedStatus()) { + // Grab our current state and covariance estimates + const Eigen::VectorXd & state = filter_.getState(); + const Eigen::MatrixXd & estimate_error_covariance = + filter_.getEstimateErrorCovariance(); + + // Convert from roll, pitch, and yaw back to quaternion for + // orientation values + tf2::Quaternion quat; + quat.setRPY( + state(StateMemberRoll), state(StateMemberPitch), + state(StateMemberYaw)); + + // Fill out the message + message->pose.pose.position.x = state(StateMemberX); + message->pose.pose.position.y = state(StateMemberY); + message->pose.pose.position.z = state(StateMemberZ); + message->pose.pose.orientation.x = quat.x(); + message->pose.pose.orientation.y = quat.y(); + message->pose.pose.orientation.z = quat.z(); + message->pose.pose.orientation.w = quat.w(); + message->twist.twist.linear.x = state(StateMemberVx); + message->twist.twist.linear.y = state(StateMemberVy); + message->twist.twist.linear.z = state(StateMemberVz); + message->twist.twist.angular.x = state(StateMemberVroll); + message->twist.twist.angular.y = state(StateMemberVpitch); + message->twist.twist.angular.z = state(StateMemberVyaw); + + // Our covariance matrix layout doesn't quite match + for (size_t i = 0; i < POSE_SIZE; i++) { + for (size_t j = 0; j < POSE_SIZE; j++) { + message->pose.covariance[POSE_SIZE * i + j] = + estimate_error_covariance(i, j); + } + } + + // POSE_SIZE and TWIST_SIZE are currently the same size, but we can spare a + // few cycles to be meticulous and not index a twist covariance array on the + // size of a pose covariance array + for (size_t i = 0; i < TWIST_SIZE; i++) { + for (size_t j = 0; j < TWIST_SIZE; j++) { + message->twist.covariance[TWIST_SIZE * i + j] = + estimate_error_covariance( + i + POSITION_V_OFFSET, + j + POSITION_V_OFFSET); + } + } + + message->header.stamp = filter_.getLastMeasurementTime(); + message->header.frame_id = world_frame_id_; + message->child_frame_id = base_link_output_frame_id_; + } + + return filter_.getInitializedStatus(); +} + +bool UkfWrapper::getFilteredAccelMessage( + geometry_msgs::msg::AccelWithCovarianceStamped * message) +{ + // If the filter has received a measurement at some point... + if (filter_.getInitializedStatus()) { + // Grab our current state and covariance estimates + const Eigen::VectorXd & state = filter_.getState(); + const Eigen::MatrixXd & estimate_error_covariance = + filter_.getEstimateErrorCovariance(); + + //! Fill out the accel_msg + message->accel.accel.linear.x = state(StateMemberAx); + message->accel.accel.linear.y = state(StateMemberAy); + message->accel.accel.linear.z = state(StateMemberAz); + message->accel.accel.angular.x = angular_acceleration_.x(); + message->accel.accel.angular.y = angular_acceleration_.y(); + message->accel.accel.angular.z = angular_acceleration_.z(); + + // Fill the covariance (only the left-upper matrix since we are not + // estimating the rotational accelerations arround the axes + for (size_t i = 0; i < ACCELERATION_SIZE; i++) { + for (size_t j = 0; j < ACCELERATION_SIZE; j++) { + // We use the POSE_SIZE since the accel cov matrix of ROS is 6x6 + message->accel.covariance[POSE_SIZE * i + j] = estimate_error_covariance( + i + POSITION_A_OFFSET, j + POSITION_A_OFFSET); + } + } + for (size_t i = ACCELERATION_SIZE; i < POSE_SIZE; i++) { + for (size_t j = ACCELERATION_SIZE; j < POSE_SIZE; j++) { + // fill out the angular portion. We assume the linear and angular portions are independent. + message->accel.covariance[POSE_SIZE * i + j] = + angular_acceleration_cov_(i - ACCELERATION_SIZE, j - ACCELERATION_SIZE); + } + } + + // Fill header information + message->header.stamp = rclcpp::Time(filter_.getLastMeasurementTime()); + message->header.frame_id = base_link_output_frame_id_; + } + + return filter_.getInitializedStatus(); +} + +void UkfWrapper::imuCallback( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data, + const CallbackData & accel_callback_data) +{ + RF_DEBUG( + "------ UkfWrapper::imuCallback (" << + topic_name << ") ------\n") // << "IMU message:\n" << *msg); + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp equal to or" + " before the last filter reset, " << "this message will be ignored. This may" + "indicate an empty or bad timestamp. (message time: " << msg->header.stamp.nanosec << + ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + + + RF_DEBUG( + "Received message that preceded the most recent pose reset. " + "Ignoring..."); + + return; + } + + // As with the odometry message, we can separate out the pose- and + // twist-related variables in the IMU message and pass them to the pose and + // twist callbacks (filters) + if (pose_callback_data.update_sum_ > 0) { + // Per the IMU message specification, if the IMU does not provide + // orientation, then its first covariance value should be set to -1, and we + // should ignore that portion of the message. robot_localization allows + // users to explicitly ignore data using its parameters, but we should also + // be compliant with message specs. + if (std::abs(msg->orientation_covariance[0] + 1) < 1e-9) { + RF_DEBUG( + "Received IMU message with -1 as its first covariance value for " + "orientation. " + "Ignoring orientation..."); + } else { + // Extract the pose (orientation) data, pass it to its filter + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pos_ptr = + std::make_shared(); + pos_ptr->header = msg->header; + pos_ptr->pose.pose.orientation = msg->orientation; + + // Copy the covariance for roll, pitch, and yaw + for (size_t i = 0; i < ORIENTATION_SIZE; i++) { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) { + pos_ptr->pose.covariance[POSE_SIZE * (i + ORIENTATION_SIZE) + + (j + ORIENTATION_SIZE)] = + msg->orientation_covariance[ORIENTATION_SIZE * i + j]; + } + } + + // IMU data gets handled a bit differently, since the message is ambiguous + // and has only a single frame_id, even though the data in it is reported + // in two different frames. As we assume users will specify a base_link to + // imu transform, we make the target and child frame base_link_frame_id_ and + // tell the poseCallback that it is working with IMU data. This will cause + // it to apply different logic to the data. + poseCallback( + pos_ptr, pose_callback_data, base_link_frame_id_, + base_link_frame_id_, true); + } + } + + if (twist_callback_data.update_sum_ > 0) { + // Ignore rotational velocity if the first covariance value is -1 + if (std::abs(msg->angular_velocity_covariance[0] + 1) < 1e-9) { + RF_DEBUG( + "Received IMU message with -1 as its first covariance value for " + "angular " + "velocity. Ignoring angular velocity..."); + } else { + // Repeat for velocity + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist_ptr = + std::make_shared(); + twist_ptr->header = msg->header; + twist_ptr->twist.twist.angular = msg->angular_velocity; + + // Copy the covariance + for (size_t i = 0; i < ORIENTATION_SIZE; i++) { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) { + twist_ptr->twist.covariance[TWIST_SIZE * (i + ORIENTATION_SIZE) + + (j + ORIENTATION_SIZE)] = + msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j]; + } + } + + twistCallback(twist_ptr, twist_callback_data, base_link_frame_id_); + } + } + + if (accel_callback_data.update_sum_ > 0) { + // Ignore linear acceleration if the first covariance value is -1 + if (std::abs(msg->linear_acceleration_covariance[0] + 1) < 1e-9) { + RF_DEBUG( + "Received IMU message with -1 as its first covariance value for " + "linear " + "acceleration. Ignoring linear acceleration..."); + } else { + // Pass the message on + accelerationCallback(msg, accel_callback_data, base_link_frame_id_); + } + } + + RF_DEBUG("\n----- /UkfWrapper::imuCallback (" << topic_name << ") ------\n"); +} + +void UkfWrapper::integrateMeasurements(const rclcpp::Time & current_time) +{ + RF_DEBUG( + "------ UkfWrapper::integrateMeasurements ------\n\n" + "Integration time is " << + std::setprecision(20) << filter_utilities::toSec(current_time) << + "\n" << + measurement_queue_.size() << " measurements in queue.\n"); + + bool predict_to_current_time = predict_to_current_time_; + + // If we have any measurements in the queue, process them + if (!measurement_queue_.empty()) { + // Check if the first measurement we're going to process is older than the + // filter's last measurement. This means we have received an out-of-sequence + // message (one with an old timestamp), and we need to revert both the + // filter state and measurement queue to the first state that preceded the + // time stamp of our first measurement. + const MeasurementPtr & first_measurement = measurement_queue_.top(); + int restored_measurement_count = 0; + if (smooth_lagged_data_ && + first_measurement->time_ < filter_.getLastMeasurementTime()) + { + RF_DEBUG( + "Received a measurement that was " << + filter_utilities::toSec( + filter_.getLastMeasurementTime() - + first_measurement->time_) << + " seconds in the past. Reverting filter state and " + "measurement queue..."); + + int original_count = static_cast(measurement_queue_.size()); + const rclcpp::Time first_measurement_time = first_measurement->time_; + const std::string first_measurement_topic = + first_measurement->topic_name_; + // revertTo may invalidate first_measurement + if (!revertTo(first_measurement_time - rclcpp::Duration(1ns))) { + RF_DEBUG( + "ERROR: history interval is too small to revert to time " << + filter_utilities::toSec(first_measurement_time) << "\n"); + // ROS_WARN_STREAM_DELAYED_THROTTLE(history_length_, + // "Received old measurement for topic " << first_measurement_topic << + // ", but history interval is insufficiently sized. " + // "Measurement time is " << std::setprecision(20) << + // first_measurement_time << + // ", current time is " << current_time << + // ", history length is " << history_length_ << "."); + restored_measurement_count = 0; + } + + restored_measurement_count = + static_cast(measurement_queue_.size()) - original_count; + } + + while (!measurement_queue_.empty() && rclcpp::ok()) { + MeasurementPtr measurement = measurement_queue_.top(); + + // If we've reached a measurement that has a time later than now, it + // should wait until a future iteration. Since measurements are stored in + // a priority queue, all remaining measurements will be in the future. + if (current_time < measurement->time_) { + break; + } + + measurement_queue_.pop(); + + // When we receive control messages, we call this directly in the control + // callback. However, we also associate a control with each sensor message + // so that we can support lagged smoothing. As we cannot guarantee that + // the new control callback will fire before a new measurement, we should + // only perform this operation if we are processing messages from the + // history. Otherwise, we may get a new measurement, store the "old" + // latest control, then receive a control, call setControl, and then + // overwrite that value with this one (i.e., with the "old" control we + // associated with the measurement). + if (use_control_ && restored_measurement_count > 0) { + filter_.setControl( + measurement->latest_control_, + measurement->latest_control_time_); + restored_measurement_count--; + } + + auto previous_state = filter_.getState(); + auto previous_covar = filter_.getEstimateErrorCovariance(); + auto last_measurement_time = filter_.getLastMeasurementTime(); + + // This will call predict and, if necessary, correct + filter_.processMeasurement(*(measurement.get())); + + // Store old states and measurements if we're smoothing + if (smooth_lagged_data_) { + // Invariant still holds: measurementHistoryDeque_.back().time_ < + // measurement_queue_.top().time_ + measurement_history_.push_back(measurement); + + // We should only save the filter state once per unique timstamp + if (measurement_queue_.empty() || + measurement_queue_.top()->time_ != + filter_.getLastMeasurementTime()) + { + saveFilterState(filter_); + } + } + } + } else if (filter_.getInitializedStatus()) { + // In the event that we don't get any measurements for a long time, + // we still need to continue to estimate our state. Therefore, we + // should project the state forward here. + rclcpp::Duration last_update_delta = + current_time - filter_.getLastMeasurementTime(); + + // If we get a large delta, then continuously predict until + if (last_update_delta >= filter_.getSensorTimeout()) { + predict_to_current_time = true; + + RF_DEBUG( + "Sensor timeout! Last measurement time was " << + filter_utilities::toSec(filter_.getLastMeasurementTime()) << + ", current time is " << filter_utilities::toSec(current_time) << + ", delta is " << filter_utilities::toSec(last_update_delta) << + "\n"); + } + } else { + RF_DEBUG("Filter not yet initialized.\n"); + } + + if (filter_.getInitializedStatus() && predict_to_current_time) { + rclcpp::Duration last_update_delta = + current_time - filter_.getLastMeasurementTime(); + + filter_.validateDelta(last_update_delta); + filter_.predict(current_time, last_update_delta); + + // Update the last measurement time and last update time + filter_.setLastMeasurementTime( + filter_.getLastMeasurementTime() + + last_update_delta); + } + + RF_DEBUG("\n----- /UkfWrapper::integrateMeasurements ------\n"); +} + +void UkfWrapper::differentiateMeasurements(const rclcpp::Time & current_time) +{ + if (filter_.getInitializedStatus()) { + const double time_now = filter_utilities::toSec(current_time); + const double dt = time_now - last_diff_time_; + const Eigen::VectorXd & state = filter_.getState(); + tf2::Vector3 new_state_twist_rot( + state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + angular_acceleration_ = (new_state_twist_rot - last_state_twist_rot_) / dt; + const Eigen::MatrixXd & cov = filter_.getEstimateErrorCovariance(); + for (size_t i = 0; i < ORIENTATION_SIZE; i++) { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) { + angular_acceleration_cov_(i, j) = + cov(i + ORIENTATION_V_OFFSET, j + ORIENTATION_V_OFFSET) * 2. / + ( dt * dt ); + } + } + last_state_twist_rot_ = new_state_twist_rot; + last_diff_time_ = time_now; + } +} + +void UkfWrapper::loadParams() +{ + // Get the plugin name, if any + const std::string param_prefix = plugin_name_.empty() ? "" : plugin_name_ + "."; + + // Get callback_group + auto rt_cbg = parent_node_->get_real_time_cbg(); + rclcpp::SubscriptionOptions options; + options.callback_group = rt_cbg; + + double alpha = parent_node_->declare_parameter(param_prefix + "alpha", 0.001); + double kappa = parent_node_->declare_parameter(param_prefix + "kappa", 0.0); + double beta = parent_node_->declare_parameter(param_prefix + "beta", 2.0); + filter_.setConstants(alpha, kappa, beta); + + /* For diagnostic purposes, collect information about how many different + * sources are measuring each absolute pose variable and do not have + * differential integration enabled. + */ + std::map abs_pose_var_counts; + abs_pose_var_counts[StateMemberX] = 0; + abs_pose_var_counts[StateMemberY] = 0; + abs_pose_var_counts[StateMemberZ] = 0; + abs_pose_var_counts[StateMemberRoll] = 0; + abs_pose_var_counts[StateMemberPitch] = 0; + abs_pose_var_counts[StateMemberYaw] = 0; + + // Same for twist variables + std::map twist_var_counts; + twist_var_counts[StateMemberVx] = 0; + twist_var_counts[StateMemberVy] = 0; + twist_var_counts[StateMemberVz] = 0; + twist_var_counts[StateMemberVroll] = 0; + twist_var_counts[StateMemberVpitch] = 0; + twist_var_counts[StateMemberVyaw] = 0; + + // Determine if we'll be printing diagnostic information + print_diagnostics_ = parent_node_->declare_parameter(param_prefix + "print_diagnostics", false); + + // Check for custom gravitational acceleration value + gravitational_acceleration_ = parent_node_->declare_parameter( + param_prefix + + "gravitational_acceleration", + gravitational_acceleration_); + + // Grab the debug param. If true, the node will produce a LOT of output. + bool debug = parent_node_->declare_parameter(param_prefix + "debug", false); + std::string debug_out_file = "robot_localization_debug.txt"; + if (debug) { + try { + debug_out_file = parent_node_->declare_parameter(param_prefix + "debug_out_file", + debug_out_file); + debug_stream_.open(debug_out_file.c_str()); + + // Make sure we succeeded + if (debug_stream_.is_open()) { + filter_.setDebug(debug, &debug_stream_); + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "UkfWrapper::loadParams() - unable to create debug output file " << debug_out_file); + } + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "UkfWrapper::loadParams() - unable to create debug output file " << debug_out_file << + ". Error was " << e.what()); + } + } + + // Frame configuration comes from Easynav TFInfo; no frame params declared here + + const auto & tf_info = easynav::RTTFBuffer::getInstance()->get_tf_info(); + + map_frame_id_ = tf_info.map_frame; + odom_frame_id_ = tf_info.odom_frame; + base_link_frame_id_ = tf_info.robot_frame; + // World frame comes from Easynav TFInfo configuration + world_frame_id_ = tf_info.map_frame; + + base_link_output_frame_id_ = base_link_frame_id_; + + /* + * These parameters are designed to enforce compliance with REP-105: + * http://www.ros.org/reps/rep-0105.html + * When fusing absolute position data from sensors such as GPS, the state + * estimate can undergo discrete jumps. According to REP-105, we want three + * coordinate frames: map, odom, and base_link. The map frame can have + * discontinuities, but is the frame with the most accurate position estimate + * for the robot and should not suffer from drift. The odom frame drifts over + * time, but is guaranteed to be continuous and is accurate enough for local + * planning and navigation. The base_link frame is affixed to the robot. The + * intention is that some odometry source broadcasts the odom->base_link + * transform. The localization software should broadcast map->base_link. + * However, tf does not allow multiple parents for a coordinate frame, so + * we must *compute* map->base_link, but then use the existing odom->base_link + * transform to compute *and broadcast* map->odom. + * + * The state estimation nodes in robot_localization therefore have two + * "modes." If your world_frame parameter value matches the map_frame + * parameter value, then robot_localization will assume someone else is + * broadcasting a transform from odom_frame->base_link_frame, and it will + * compute the map_frame->odom_frame transform. Otherwise, it will simply + * compute the odom_frame->base_link_frame transform. + * + * The default is the latter behavior (broadcast of odom->base_link). + */ + + + if (map_frame_id_ == odom_frame_id_ || + odom_frame_id_ == base_link_frame_id_ || + map_frame_id_ == base_link_frame_id_ || + odom_frame_id_ == base_link_output_frame_id_ || + map_frame_id_ == base_link_output_frame_id_) + { + RCLCPP_ERROR( + parent_node_->get_logger(), + "Invalid frame configuration! The values for map_frame, " + "odom_frame, and base_link_frame must be unique. If using a base_link_frame_output " + "values, it must not match the map_frame or odom_frame."); + } + + if (!tf_prefix_.empty()) { + // Append the tf prefix in a tf2-friendly manner + filter_utilities::appendPrefix(tf_prefix_, map_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, odom_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, base_link_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, base_link_output_frame_id_); + filter_utilities::appendPrefix(tf_prefix_, world_frame_id_); + } + + // Whether we're publshing the world_frame->base_link_frame transform + publish_transform_ = parent_node_->declare_parameter(param_prefix + "publish_tf", true); + + // Whether we're publishing the acceleration state transform + publish_acceleration_ = parent_node_->declare_parameter(param_prefix + "publish_acceleration", + false); + + // Whether we'll allow old measurements to cause a re-publication of the updated state + permit_corrected_publication_ = parent_node_->declare_parameter(param_prefix + + "permit_corrected_publication", false); + + // Transform future dating + double offset_tmp = parent_node_->declare_parameter(param_prefix + "transform_time_offset", 0.0); + tf_time_offset_ = rclcpp::Duration::from_seconds(offset_tmp); + + // Transform timeout + double timeout_tmp = parent_node_->declare_parameter(param_prefix + "transform_timeout", 0.0); + tf_timeout_ = rclcpp::Duration::from_seconds(timeout_tmp); + + // Update frequency and sensor timeout + frequency_ = parent_node_->declare_parameter(param_prefix + "frequency", 30.0); + + predict_to_current_time_ = parent_node_->declare_parameter(param_prefix + + "predict_to_current_time", false); + + sensor_timeout_ = + rclcpp::Duration::from_seconds(parent_node_->declare_parameter(param_prefix + "sensor_timeout", + 1.0 / frequency_)); + filter_.setSensorTimeout(sensor_timeout_); + + // Determine if we're in 2D mode + two_d_mode_ = parent_node_->declare_parameter(param_prefix + "two_d_mode", false); + + // Smoothing window size + smooth_lagged_data_ = parent_node_->declare_parameter(param_prefix + "smooth_lagged_data", false); + double history_length_double = parent_node_->declare_parameter(param_prefix + "history_length", + 0.0); + + if (!smooth_lagged_data_ && std::abs(history_length_double) > 0) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Filter history interval of " << history_length_double << + " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed."); + } + + if (smooth_lagged_data_ && history_length_double < 0) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Negative history interval of " << history_length_double << " specified. Absolute value " + "will be assumed."); + } + + history_length_ = rclcpp::Duration::from_seconds(std::abs(history_length_double)); + + // Whether we reset filter on jump back in time + reset_on_time_jump_ = parent_node_->declare_parameter(param_prefix + "reset_on_time_jump", false); + + // Determine if we're using a control term + double control_timeout = sensor_timeout_.seconds(); + std::vector control_update_vector; + std::vector acceleration_limits; + std::vector acceleration_gains; + std::vector deceleration_limits; + std::vector deceleration_gains; + + use_control_ = parent_node_->declare_parameter(param_prefix + "use_control", false); + stamped_control_ = parent_node_->declare_parameter(param_prefix + "stamped_control", true); + control_timeout = parent_node_->declare_parameter(param_prefix + "control_timeout", 0.0); + + if (use_control_) { + parent_node_->declare_parameter(param_prefix + "control_config", rclcpp::PARAMETER_BOOL_ARRAY); + if (parent_node_->get_parameter(param_prefix + "control_config", control_update_vector)) { + if (control_update_vector.size() != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Control configuration must be of size " << + TWIST_SIZE << ". Provided config was of size " << control_update_vector.size() << + ". No control term will be used."); + use_control_ = false; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but control_config is missing. No control term will be " + "used."); + use_control_ = false; + } + + parent_node_->declare_parameter(param_prefix + "acceleration_limits", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "acceleration_limits", acceleration_limits)) { + if (acceleration_limits.size() != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Acceleration configuration must be of size " << TWIST_SIZE << + ". Provided config was of size " << acceleration_limits.size() << + ". No control term will be used."); + use_control_ = false; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but acceleration_limits is missing. Will use default " + "values."); + acceleration_limits.resize(TWIST_SIZE, 1.0); + } + + parent_node_->declare_parameter(param_prefix + "acceleration_gains", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "acceleration_gains", acceleration_gains)) { + const int size = acceleration_gains.size(); + if (size != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Acceleration gain configuration must be of size " << TWIST_SIZE << ". Provided config " + "was of size " << size << ". All gains will be assumed to be 1."); + std::fill_n( + acceleration_gains.begin(), std::min(size, TWIST_SIZE), + 1.0); + acceleration_gains.resize(TWIST_SIZE, 1.0); + } + } + + parent_node_->declare_parameter(param_prefix + "deceleration_limits", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "deceleration_limits", deceleration_limits)) { + if (deceleration_limits.size() != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Deceleration configuration must be of size " << TWIST_SIZE << ". Provided config was " + "of size " << deceleration_limits.size() << ". No control term will be used."); + use_control_ = false; + } + } else { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but no deceleration_limits specified. Will use acceleration " + "limits."); + deceleration_limits = acceleration_limits; + } + + parent_node_->declare_parameter(param_prefix + "deceleration_gains", + rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "deceleration_gains", deceleration_gains)) { + const int size = deceleration_gains.size(); + if (size != TWIST_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Deceleration gain configuration must be of size " << TWIST_SIZE << ". Provided config " + "was of size " << size << ". All gains will be assumed to be 1."); + std::fill_n( + deceleration_gains.begin(), std::min(size, TWIST_SIZE), + 1.0); + deceleration_gains.resize(TWIST_SIZE, 1.0); + } + } else { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but no deceleration_gains specified. Will use acceleration " + "gains."); + deceleration_gains = acceleration_gains; + } + } else { + control_update_vector.resize(TWIST_SIZE, 0); + acceleration_limits.resize(TWIST_SIZE, 1.0); + acceleration_gains.resize(TWIST_SIZE, 1.0); + deceleration_limits.resize(TWIST_SIZE, 1.0); + deceleration_gains.resize(TWIST_SIZE, 1.0); + } + + bool dynamic_process_noise_covariance = parent_node_->declare_parameter(param_prefix + + "dynamic_process_noise_covariance", false); + filter_.setUseDynamicProcessNoiseCovariance( + dynamic_process_noise_covariance); + + std::vector initial_state; + parent_node_->declare_parameter(param_prefix + "initial_state", rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(param_prefix + "initial_state", initial_state)) { + if (initial_state.size() != STATE_SIZE) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Initial state must be of size " << STATE_SIZE << ". Provided config was of size " << + initial_state.size() << ". The initial state will be ignored."); + } else { + Eigen::Map eigen_state(initial_state.data(), + initial_state.size()); + filter_.setState(eigen_state); + } + } + + // Check if the filter should start or not + disabled_at_startup_ = parent_node_->declare_parameter(param_prefix + "disabled_at_startup", + false); + enabled_ = !disabled_at_startup_; + + // Debugging writes to file + RF_DEBUG( + std::boolalpha << + "tf_prefix is " << tf_prefix_ << + "\nmap_frame is " << map_frame_id_ << + "\nodom_frame is " << odom_frame_id_ << + "\nbase_link_frame is " << base_link_frame_id_ << + "\nbase_link_output_frame is " << base_link_output_frame_id_ << + "\nworld_frame is " << world_frame_id_ << + "\ntransform_time_offset is " << filter_utilities::toSec(tf_time_offset_) << + "\ntransform_timeout is " << filter_utilities::toSec(tf_timeout_) << + "\nfrequency is " << frequency_ << + "\nsensor_timeout is " << filter_utilities::toSec(filter_.getSensorTimeout()) << + "\ntwo_d_mode is " << (two_d_mode_ ? "true" : "false") << + "\nsmooth_lagged_data is " << (smooth_lagged_data_ ? "true" : "false") << + "\nhistory_length is " << filter_utilities::toSec(history_length_) << + "\nuse_control is " << use_control_ << + "\nstamped_control_is " << stamped_control_ << + "\ncontrol_config is " << control_update_vector << + "\ncontrol_timeout is " << control_timeout << + "\nacceleration_limits are " << acceleration_limits << + "\nacceleration_gains are " << acceleration_gains << + "\ndeceleration_limits are " << deceleration_limits << + "\ndeceleration_gains are " << deceleration_gains << + "\ninitial state is " << filter_.getState() << + "\ndynamic_process_noise_covariance is " << dynamic_process_noise_covariance << + "\npermit_corrected_publication is " << permit_corrected_publication_ << + "\nprint_diagnostics is " << print_diagnostics_ << "\n"); + + // Create a subscriber for manually setting/resetting pose + set_pose_sub_ = + parent_node_->create_subscription( + "set_pose", rclcpp::QoS(1), + std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options); + + // Create a service for manually setting/resetting pose + set_pose_service_ = + parent_node_->create_service( + "set_pose", std::bind( + &UkfWrapper::setPoseSrvCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Create a service for manually enabling the filter + enable_filter_srv_ = + parent_node_->create_service( + "~/enable", std::bind( + &UkfWrapper::enableFilterSrvCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Create a service for manually setting/resetting pose + reset_srv_ = + parent_node_->create_service( + "reset", std::bind( + &UkfWrapper::resetSrvCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Create a service for toggling processing new measurements while still + // publishing + toggle_filter_processing_srv_ = + parent_node_->create_service( + "~/toggle", std::bind( + &UkfWrapper::toggleFilterProcessingCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // Init the last measurement time so we don't get a huge initial delta + filter_.setLastMeasurementTime(parent_node_->now()); + + // Now pull in each topic to which we want to subscribe. + // Start with odom. + size_t topic_ind = 0; + bool more_params = false; + do { + // Build the string in the form of "odomX", where X is the odom topic + // number, then check if we have any parameters with that value. Users need + // to make sure they don't have gaps in their configs (e.g., odom0 and then + // odom2) + std::stringstream ss; + ss << "odom" << topic_ind++; + std::string odom_topic_name = ss.str(); + std::string odom_topic; + parent_node_->declare_parameter(param_prefix + odom_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + odom_topic_name, parameter)) { + more_params = true; + odom_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + // Determine if we want to integrate this sensor differentially + bool differential = parent_node_->declare_parameter(param_prefix + + odom_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter(param_prefix + odom_topic_name + + std::string("_relative"), false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << odom_topic_name << "_differential" << " and " << odom_topic_name << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + // Consider odometry transformation from the child_frame_id instead of the base_link_frame_id + bool pose_use_child_frame = parent_node_->declare_parameter(param_prefix + + odom_topic_name + std::string("_pose_use_child_frame"), false); + + // Check for pose rejection threshold + double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + odom_topic_name + + std::string("_pose_rejection_threshold"), + std::numeric_limits::max()); + + // Check for twist rejection threshold + double twist_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + odom_topic_name + + std::string("_twist_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter(param_prefix + + odom_topic_name + + std::string("_queue_size"), 10); + + // Now pull in its boolean update vector configuration. Create separate + // vectors for pose and twist data, and then zero out the opposite values + // in each vector (no pose data in the twist update vector and + // vice-versa). + std::vector update_vec = loadUpdateConfig(odom_topic_name); + std::vector pose_update_vec = update_vec; + std::fill( + pose_update_vec.begin() + POSITION_V_OFFSET, + pose_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::vector twist_update_vec = update_vec; + std::fill( + twist_update_vec.begin() + POSITION_OFFSET, + twist_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + + int pose_update_sum = + std::accumulate(pose_update_vec.begin(), pose_update_vec.end(), 0); + int twist_update_sum = + std::accumulate(twist_update_vec.begin(), twist_update_vec.end(), 0); + + const CallbackData pose_callback_data( + odom_topic_name + "_pose", pose_update_vec, pose_update_sum, + differential, relative, pose_use_child_frame, pose_mahalanobis_thresh); + + const CallbackData twist_callback_data( + odom_topic_name + "_twist", twist_update_vec, twist_update_sum, false, + false, false, twist_mahalanobis_thresh); + + // Store the odometry topic subscribers so they don't go out of scope. + if (pose_update_sum + twist_update_sum > 0) { + std::function)> + odom_callback = std::bind( + &UkfWrapper::odometryCallback, this, + std::placeholders::_1, odom_topic_name, + pose_callback_data, twist_callback_data); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + topic_subs_.push_back( + parent_node_->create_subscription( + odom_topic, custom_qos, + odom_callback, options)); + } else { + std::stringstream stream; + stream << odom_topic << " is listed as an input topic, but all update " + "variables are false"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + odom_topic + "_configuration", stream.str(), true); + } + + if (pose_update_sum > 0) { + if (differential) { + twist_var_counts[StateMemberVx] += pose_update_vec[StateMemberX]; + twist_var_counts[StateMemberVy] += pose_update_vec[StateMemberY]; + twist_var_counts[StateMemberVz] += pose_update_vec[StateMemberZ]; + twist_var_counts[StateMemberVroll] += + pose_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + pose_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += pose_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberX] += pose_update_vec[StateMemberX]; + abs_pose_var_counts[StateMemberY] += pose_update_vec[StateMemberY]; + abs_pose_var_counts[StateMemberZ] += pose_update_vec[StateMemberZ]; + abs_pose_var_counts[StateMemberRoll] += + pose_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + pose_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + pose_update_vec[StateMemberYaw]; + } + } + + if (twist_update_sum > 0) { + twist_var_counts[StateMemberVx] += twist_update_vec[StateMemberVx]; + twist_var_counts[StateMemberVy] += twist_update_vec[StateMemberVx]; + twist_var_counts[StateMemberVz] += twist_update_vec[StateMemberVz]; + twist_var_counts[StateMemberVroll] += + twist_update_vec[StateMemberVroll]; + twist_var_counts[StateMemberVpitch] += + twist_update_vec[StateMemberVpitch]; + twist_var_counts[StateMemberVyaw] += twist_update_vec[StateMemberVyaw]; + } + + RF_DEBUG( + "Subscribed to " << + odom_topic << " (" << odom_topic_name << ")\n\t" << + odom_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << odom_topic_name << + "_pose_rejection_threshold is " << pose_mahalanobis_thresh << + "\n\t" << odom_topic_name << "_twist_rejection_threshold is " << + twist_mahalanobis_thresh << "\n\t" << odom_topic_name << + " pose update vector is " << pose_update_vec << "\t" << + odom_topic_name << " twist update vector is " << + twist_update_vec); + } + } while (more_params); + + // Repeat for pose + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "pose" << topic_ind++; + std::string pose_topic_name = ss.str(); + std::string pose_topic; + parent_node_->declare_parameter(param_prefix + pose_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + pose_topic_name, parameter)) { + more_params = true; + pose_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + bool differential = parent_node_->declare_parameter(param_prefix + + pose_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter(param_prefix + + pose_topic_name + std::string("_relative"), + false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << pose_topic_name << "_differential" << " and " << pose_topic_name << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + // Check for pose rejection threshold + double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + pose_topic_name + + std::string("_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter(param_prefix + + pose_topic_name + + std::string("_queue_size"), 10); + + // Pull in the sensor's config, zero out values that are invalid for the + // pose type + std::vector pose_update_vec = loadUpdateConfig(pose_topic_name); + std::fill( + pose_update_vec.begin() + POSITION_V_OFFSET, + pose_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::fill( + pose_update_vec.begin() + POSITION_A_OFFSET, + pose_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, + 0); + + int pose_update_sum = + std::accumulate(pose_update_vec.begin(), pose_update_vec.end(), 0); + + if (pose_update_sum > 0) { + const CallbackData callback_data(pose_topic_name, pose_update_vec, + pose_update_sum, differential, + relative, false, pose_mahalanobis_thresh); + + std::function)> + pose_callback = + std::bind( + &UkfWrapper::poseCallback, this, std::placeholders::_1, + callback_data, world_frame_id_, base_link_frame_id_, false); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + + topic_subs_.push_back( + parent_node_->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, custom_qos, pose_callback, options)); + + if (differential) { + twist_var_counts[StateMemberVx] += pose_update_vec[StateMemberX]; + twist_var_counts[StateMemberVy] += pose_update_vec[StateMemberY]; + twist_var_counts[StateMemberVz] += pose_update_vec[StateMemberZ]; + twist_var_counts[StateMemberVroll] += + pose_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + pose_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += pose_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberX] += pose_update_vec[StateMemberX]; + abs_pose_var_counts[StateMemberY] += pose_update_vec[StateMemberY]; + abs_pose_var_counts[StateMemberZ] += pose_update_vec[StateMemberZ]; + abs_pose_var_counts[StateMemberRoll] += + pose_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + pose_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + pose_update_vec[StateMemberYaw]; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << pose_topic << " is listed as an input topic, but all pose update " + "variables are false"); + } + + RF_DEBUG( + "Subscribed to " << pose_topic << " (" << pose_topic_name << ")\n\t" << + pose_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << pose_topic_name << + "_rejection_threshold is " << pose_mahalanobis_thresh << + "\n\t" << pose_topic_name << " update vector is " << + pose_update_vec); + } + } while (more_params); + + // Repeat for gps + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "gps" << topic_ind++; + std::string gps_topic_name = ss.str(); + std::string gps_topic; + parent_node_->declare_parameter(param_prefix + gps_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + gps_topic_name, parameter)) { + more_params = true; + gps_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + bool differential = parent_node_->declare_parameter(param_prefix + + gps_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter(param_prefix + + gps_topic_name + std::string("_relative"), + false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << gps_topic_name << "_differential" << " and " << gps_topic_name << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + // Check for gps rejection threshold + double gps_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + gps_topic_name + + std::string("_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + // int queue_size = parent_node_->declare_parameter(param_prefix + + // gps_topic_name + + // std::string("_queue_size"), 10); + + // Pull in the sensor's config, zero out values that are invalid for the + // gps type + std::vector gps_update_vec = loadUpdateConfig(gps_topic_name); + std::fill( + gps_update_vec.begin() + POSITION_V_OFFSET, + gps_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::fill( + gps_update_vec.begin() + POSITION_A_OFFSET, + gps_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, + 0); + + int gps_update_sum = + std::accumulate(gps_update_vec.begin(), gps_update_vec.end(), 0); + + if (gps_update_sum > 0) { + const CallbackData callback_data(gps_topic_name, gps_update_vec, + gps_update_sum, differential, + relative, false, gps_mahalanobis_thresh); + + // Block Commented as it is handled in update_rt + + // std::function)> + // gps_callback = + // std::bind( + // &UkfWrapper::gpsCallback, this, std::placeholders::_1, + // callback_data, world_frame_id_, base_link_frame_id_, false); + + // auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + + // topic_subs_.push_back( + // parent_node_->create_subscription< + // geometry_msgs::msg::gpsWithCovarianceStamped>( + // gps_topic, custom_qos, gps_callback)); + + gps_callbackData_arr_.push_back(callback_data); + + if (differential) { + twist_var_counts[StateMemberVx] += gps_update_vec[StateMemberX]; + twist_var_counts[StateMemberVy] += gps_update_vec[StateMemberY]; + twist_var_counts[StateMemberVz] += gps_update_vec[StateMemberZ]; + twist_var_counts[StateMemberVroll] += + gps_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + gps_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += gps_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberX] += gps_update_vec[StateMemberX]; + abs_pose_var_counts[StateMemberY] += gps_update_vec[StateMemberY]; + abs_pose_var_counts[StateMemberZ] += gps_update_vec[StateMemberZ]; + abs_pose_var_counts[StateMemberRoll] += + gps_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + gps_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + gps_update_vec[StateMemberYaw]; + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << gps_topic << " is listed as an input topic, but all pose update " + "variables are false"); + } + + RF_DEBUG( + "Subscribed to " << gps_topic << " (" << gps_topic_name << ")\n\t" << + gps_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << gps_topic_name << + "_rejection_threshold is " << gps_mahalanobis_thresh << + "\n\t" << gps_topic_name << " update vector is " << + gps_update_vec); + } + } while (more_params); + + // Repeat for twist + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "twist" << topic_ind++; + std::string twist_topic_name = ss.str(); + std::string twist_topic; + parent_node_->declare_parameter(param_prefix + twist_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + twist_topic_name, parameter)) { + more_params = true; + twist_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + // Check for twist rejection threshold + double twist_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + twist_topic_name + + std::string("_rejection_threshold"), + std::numeric_limits::max()); + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter(param_prefix + + twist_topic_name + + std::string("_queue_size"), 10); + + // Pull in the sensor's config, zero out values that are invalid for the + // twist type + std::vector twist_update_vec = loadUpdateConfig(twist_topic_name); + std::fill( + twist_update_vec.begin() + POSITION_OFFSET, + twist_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + + int twist_update_sum = + std::accumulate(twist_update_vec.begin(), twist_update_vec.end(), 0); + + if (twist_update_sum > 0) { + const CallbackData callback_data(twist_topic_name, twist_update_vec, + twist_update_sum, false, false, false, + twist_mahalanobis_thresh); + + std::function)> + twist_callback = std::bind( + &UkfWrapper::twistCallback, this, + std::placeholders::_1, callback_data, + base_link_frame_id_); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + topic_subs_.push_back( + parent_node_->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, custom_qos, twist_callback, options)); + + twist_var_counts[StateMemberVx] += twist_update_vec[StateMemberVx]; + twist_var_counts[StateMemberVy] += twist_update_vec[StateMemberVy]; + twist_var_counts[StateMemberVz] += twist_update_vec[StateMemberVz]; + twist_var_counts[StateMemberVroll] += + twist_update_vec[StateMemberVroll]; + twist_var_counts[StateMemberVpitch] += + twist_update_vec[StateMemberVpitch]; + twist_var_counts[StateMemberVyaw] += twist_update_vec[StateMemberVyaw]; + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << twist_topic << " is listed as an input topic, but all twist update " + "variables are false"); + } + + RF_DEBUG( + "Subscribed to " << twist_topic << " (" << twist_topic_name << ")\n\t" << + twist_topic_name << "_rejection_threshold is " << twist_mahalanobis_thresh << + "\n\t" << twist_topic_name << " update vector is " << twist_update_vec); + } + } while (more_params); + + // Repeat for IMU + topic_ind = 0; + more_params = false; + do { + std::stringstream ss; + ss << "imu" << topic_ind++; + std::string imu_topic_name = ss.str(); + std::string imu_topic; + parent_node_->declare_parameter(param_prefix + imu_topic_name, rclcpp::PARAMETER_STRING); + + rclcpp::Parameter parameter; + if (parent_node_->get_parameter(param_prefix + imu_topic_name, parameter)) { + more_params = true; + imu_topic = parameter.as_string(); + } else { + more_params = false; + } + + if (more_params) { + bool differential = parent_node_->declare_parameter(param_prefix + + imu_topic_name + std::string("_differential"), + false); + + // Determine if we want to integrate this sensor relatively + bool relative = parent_node_->declare_parameter(param_prefix + imu_topic_name + + std::string("_relative"), false); + + if (relative && differential) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Both " << imu_topic_name << "_differential" << " and " << imu_topic_name << "_relative " + "were set to true. Using differential mode."); + + relative = false; + } + + // Check for pose rejection threshold + double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + imu_topic_name + + std::string("_pose_rejection_threshold"), + std::numeric_limits::max()); + + // Check for angular velocity rejection threshold + std::string imu_twist_rejection_name = + imu_topic_name + std::string("_twist_rejection_threshold"); + double twist_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + imu_twist_rejection_name, + std::numeric_limits::max()); + + // Check for acceleration rejection threshold + double accel_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + imu_topic_name + + std::string("_linear_acceleration_rejection_threshold"), + std::numeric_limits::max()); + + bool remove_grav_acc = parent_node_->declare_parameter(param_prefix + + imu_topic_name + + "_remove_gravitational_acceleration", + false); + remove_gravitational_acceleration_[imu_topic_name + "_acceleration"] = + remove_grav_acc; + + // Set optional custom queue size + int queue_size = parent_node_->declare_parameter(param_prefix + + imu_topic_name + + std::string("_queue_size"), 10); + + // Now pull in its boolean update vector configuration and differential + // update configuration (as this contains pose information) + std::vector update_vec = loadUpdateConfig(imu_topic_name); + + // sanity checks for update config settings + std::vector position_update_vec(update_vec.begin() + POSITION_OFFSET, + update_vec.begin() + POSITION_OFFSET + POSITION_SIZE); + int position_update_sum = std::accumulate( + position_update_vec.begin(), + position_update_vec.end(), 0); + if (position_update_sum > 0) { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "Warning: Some position entries in parameter " << imu_topic_name << "_config are set to " + "true, but the sensor_msgs/Imu message contains no positional data"); + } + std::vector linear_velocity_update_vec( + update_vec.begin() + POSITION_V_OFFSET, + update_vec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE); + int linear_velocity_update_sum = std::accumulate( + linear_velocity_update_vec.begin(), linear_velocity_update_vec.end(), + 0); + if (linear_velocity_update_sum > 0) { + RCLCPP_WARN_STREAM( + parent_node_->get_logger(), + "Warning: Some position entries in parameter " << imu_topic_name << "_config are set to " + "true, but the sensor_msgs/Imu message contains no linear velocity data"); + } + + std::vector pose_update_vec = update_vec; + // IMU message contains no information about position, filter everything + // except orientation + std::fill( + pose_update_vec.begin() + POSITION_OFFSET, + pose_update_vec.begin() + POSITION_OFFSET + POSITION_SIZE, 0); + std::fill( + pose_update_vec.begin() + POSITION_V_OFFSET, + pose_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::fill( + pose_update_vec.begin() + POSITION_A_OFFSET, + pose_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, 0); + + std::vector twist_update_vec = update_vec; + // IMU message contains no information about linear speeds, filter + // everything except angular velocity + std::fill( + twist_update_vec.begin() + POSITION_OFFSET, + twist_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + std::fill( + twist_update_vec.begin() + POSITION_V_OFFSET, + twist_update_vec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE, 0); + std::fill( + twist_update_vec.begin() + POSITION_A_OFFSET, + twist_update_vec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, 0); + + std::vector accel_update_vec = update_vec; + std::fill( + accel_update_vec.begin() + POSITION_OFFSET, + accel_update_vec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + std::fill( + accel_update_vec.begin() + POSITION_V_OFFSET, + accel_update_vec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + + int pose_update_sum = + std::accumulate(pose_update_vec.begin(), pose_update_vec.end(), 0); + int twist_update_sum = + std::accumulate(twist_update_vec.begin(), twist_update_vec.end(), 0); + int accelUpdateSum = + std::accumulate(accel_update_vec.begin(), accel_update_vec.end(), 0); + + // Check if we're using control input for any of the acceleration + // variables; turn off if so + if (control_update_vector[ControlMemberVx] && + static_cast(accel_update_vec[StateMemberAx])) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "X acceleration is being measured from IMU; X velocity control input is disabled"); + control_update_vector[ControlMemberVx] = 0; + } + if (control_update_vector[ControlMemberVy] && + static_cast(accel_update_vec[StateMemberAy])) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Y acceleration is being measured from IMU; Y velocity control input is disabled"); + control_update_vector[ControlMemberVy] = 0; + } + if (control_update_vector[ControlMemberVz] && + static_cast(accel_update_vec[StateMemberAz])) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Z acceleration is being measured from IMU; Z velocity control input is disabled"); + control_update_vector[ControlMemberVz] = 0; + } + + if (pose_update_sum + twist_update_sum + accelUpdateSum > 0) { + const CallbackData pose_callback_data( + imu_topic_name + "_pose", pose_update_vec, pose_update_sum, + differential, relative, false, pose_mahalanobis_thresh); + const CallbackData twist_callback_data( + imu_topic_name + "_twist", twist_update_vec, twist_update_sum, + differential, relative, false, twist_mahalanobis_thresh); + const CallbackData accel_callback_data( + imu_topic_name + "_acceleration", accel_update_vec, accelUpdateSum, + differential, relative, false, accel_mahalanobis_thresh); + + std::function)> + imu_callback = + std::bind( + &UkfWrapper::imuCallback, this, std::placeholders::_1, + imu_topic_name, pose_callback_data, + twist_callback_data, accel_callback_data); + + auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); + topic_subs_.push_back( + parent_node_->create_subscription( + imu_topic, custom_qos, imu_callback, options)); + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Warning: " << imu_topic << " is listed as an input topic, but all its update variables " + "are false"); + } + + if (pose_update_sum > 0) { + if (differential) { + twist_var_counts[StateMemberVroll] += + pose_update_vec[StateMemberRoll]; + twist_var_counts[StateMemberVpitch] += + pose_update_vec[StateMemberPitch]; + twist_var_counts[StateMemberVyaw] += pose_update_vec[StateMemberYaw]; + } else { + abs_pose_var_counts[StateMemberRoll] += + pose_update_vec[StateMemberRoll]; + abs_pose_var_counts[StateMemberPitch] += + pose_update_vec[StateMemberPitch]; + abs_pose_var_counts[StateMemberYaw] += + pose_update_vec[StateMemberYaw]; + } + } + + if (twist_update_sum > 0) { + twist_var_counts[StateMemberVroll] += + twist_update_vec[StateMemberVroll]; + twist_var_counts[StateMemberVpitch] += + twist_update_vec[StateMemberVpitch]; + twist_var_counts[StateMemberVyaw] += twist_update_vec[StateMemberVyaw]; + } + + RF_DEBUG( + "Subscribed to " << + imu_topic << " (" << imu_topic_name << ")\n\t" << + imu_topic_name << "_differential is " << + (differential ? "true" : "false") << "\n\t" << imu_topic_name << + "_pose_rejection_threshold is " << pose_mahalanobis_thresh << + "\n\t" << imu_topic_name << "_twist_rejection_threshold is " << + twist_mahalanobis_thresh << "\n\t" << imu_topic_name << + "_linear_acceleration_rejection_threshold is " << + accel_mahalanobis_thresh << "\n\t" << imu_topic_name << + "_remove_gravitational_acceleration is " << + (remove_grav_acc ? "true" : "false") << "\n\t" << + imu_topic_name << " pose update vector is " << pose_update_vec << + "\t" << imu_topic_name << " twist update vector is " << + twist_update_vec << "\t" << imu_topic_name << + " acceleration update vector is " << accel_update_vec); + } + } while (more_params); + + // Now that we've checked if IMU linear acceleration is being used, we can + // determine our final control parameters + if (use_control_ && std::accumulate( + control_update_vector.begin(), + control_update_vector.end(), 0) == 0) + { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "use_control is set to true, but control_config has only false values. No control term will " + "be used."); + use_control_ = false; + } + + // If we're using control, set the parameters and create the necessary + // subscribers + if (use_control_) { + latest_control_.resize(TWIST_SIZE); + latest_control_.setZero(); + + filter_.setControlParams( + control_update_vector, + rclcpp::Duration::from_seconds(control_timeout), + acceleration_limits, acceleration_gains, deceleration_limits, + deceleration_gains); + + // Select between TwistStamped or Twist control input + if(stamped_control_) { + stamped_control_sub_ = parent_node_->create_subscription( + "cmd_vel", rclcpp::QoS(1), + std::bind(&UkfWrapper::controlStampedCallback, this, std::placeholders::_1), options); + } else { + control_sub_ = parent_node_->create_subscription( + "cmd_vel", rclcpp::QoS(1), + std::bind(&UkfWrapper::controlCallback, this, std::placeholders::_1), options); + } + } + + /* Warn users about: + * 1. Multiple non-differential input sources + * 2. No absolute *or* velocity measurements for pose variables + */ + if (print_diagnostics_) { + for (int state_var = StateMemberX; state_var <= StateMemberYaw; ++state_var) { + if (abs_pose_var_counts[static_cast(state_var)] > 1) { + std::stringstream stream; + stream << abs_pose_var_counts[static_cast(state_var - + POSITION_OFFSET)] << " absolute pose inputs detected for " << + state_variable_names_[state_var] << + ". This may result in oscillations. Please ensure that your" + "variances for each measured variable are set appropriately."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + state_variable_names_[state_var] + "_configuration", + stream.str(), true); + } else if (abs_pose_var_counts[static_cast(state_var)] == 0) { + if ((static_cast(state_var) == StateMemberX && + twist_var_counts[static_cast(StateMemberVx)] == 0) || + (static_cast(state_var) == StateMemberY && + twist_var_counts[static_cast(StateMemberVy)] == 0) || + (static_cast(state_var) == StateMemberZ && + twist_var_counts[static_cast(StateMemberVz)] == 0 && + two_d_mode_ == false) || + (static_cast(state_var) == StateMemberRoll && + twist_var_counts[static_cast(StateMemberVroll)] == 0 && + two_d_mode_ == false) || (static_cast(state_var) == + StateMemberPitch && + twist_var_counts[static_cast(StateMemberVpitch)] == 0 && + two_d_mode_ == false) || (static_cast(state_var) == + StateMemberYaw && twist_var_counts[static_cast(StateMemberVyaw)] == + 0)) + { + std::stringstream stream; + stream << "Neither " << state_variable_names_[state_var] << " nor its " + "velocity is being measured. This will result in unbounded" + "error growth and erratic filter behavior."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + state_variable_names_[state_var] + "_configuration", + stream.str(), true); + } + } + } + } + + auto load_covariance = [this](const std::string & parameter, Eigen::MatrixXd & covariance) + { + covariance.setZero(); + std::vector covar_flat; + + parent_node_->declare_parameter(parameter, rclcpp::PARAMETER_DOUBLE_ARRAY); + if (parent_node_->get_parameter(parameter, covar_flat)) { + if (covar_flat.size() == STATE_SIZE) { + RCLCPP_INFO_STREAM( + parent_node_->get_logger(), "Detected a " << parameter << " parameter with " + "length " << STATE_SIZE << ". Assuming diagonal values specified."); + covariance.diagonal() = Eigen::VectorXd::Map(covar_flat.data(), STATE_SIZE); + } else if (covar_flat.size() == STATE_SIZE * STATE_SIZE) { + covariance = Eigen::MatrixXd::Map(covar_flat.data(), STATE_SIZE, STATE_SIZE); + } else { + std::string error = "Invalid " + parameter + " specified. Expected a length of " + + std::to_string(STATE_SIZE) + " or " + std::to_string(STATE_SIZE * STATE_SIZE) + + ", received length " + std::to_string(covar_flat.size()); + RCLCPP_FATAL_STREAM(parent_node_->get_logger(), error); + throw std::invalid_argument(error); + } + return true; + } + return false; + }; + + if (load_covariance(param_prefix + "process_noise_covariance", process_noise_covariance_)) { + RF_DEBUG("Process noise covariance is:\n" << process_noise_covariance_ << "\n"); + filter_.setProcessNoiseCovariance(process_noise_covariance_); + } + + if (load_covariance(param_prefix + "initial_estimate_covariance", + initial_estimate_error_covariance_)) + { + RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n"); + filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); + } +} + +void UkfWrapper::odometryCallback( + const nav_msgs::msg::Odometry::SharedPtr msg, + const std::string & topic_name, + const CallbackData & pose_callback_data, + const CallbackData & twist_callback_data) +{ + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << + "The " << topic_name << + " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad " + "timestamp. (message time: " << + filter_utilities::toSec(msg->header.stamp) << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + RF_DEBUG( + "Received message that preceded the most recent pose reset. " + "Ignoring..."); + + return; + } + + RF_DEBUG( + "------ UkfWrapper::odometryCallback (" << + topic_name << ") ------\n") // << "Odometry message:\n" << *msg); + + if (pose_callback_data.update_sum_ > 0) { + // Grab the pose portion of the message and pass it to the poseCallback + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pos_ptr = + std::make_shared(); + pos_ptr->header = msg->header; + pos_ptr->pose = msg->pose; // Entire pose object, also copies covariance + + if (pose_callback_data.pose_use_child_frame_) { + poseCallback(pos_ptr, pose_callback_data, world_frame_id_, msg->child_frame_id, false); + } else { + poseCallback(pos_ptr, pose_callback_data, world_frame_id_, base_link_frame_id_, false); + } + } + + if (twist_callback_data.update_sum_ > 0) { + // Grab the twist portion of the message and pass it to the twistCallback + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist_ptr = + std::make_shared(); + twist_ptr->header = msg->header; + twist_ptr->header.frame_id = msg->child_frame_id; + twist_ptr->twist = + msg->twist; // Entire twist object, also copies covariance + + twistCallback(twist_ptr, twist_callback_data, base_link_frame_id_); + } + + RF_DEBUG( + "\n----- /UkfWrapper::odometryCallback (" << topic_name << + ") ------\n"); +} + +void UkfWrapper::poseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame, + const std::string & pose_source_frame, const bool imu_data) +{ + const std::string & topic_name = callback_data.topic_name_; + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << + "The " << topic_name << + " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad " + "timestamp. (message time: " << + filter_utilities::toSec(msg->header.stamp) << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + return; + } + + RF_DEBUG( + "------ UkfWrapper::poseCallback (" << topic_name << ") ------\n" + "Pose message:\n" << geometry_msgs::msg::to_yaml(*msg)); + + // Put the initial value in the lastMessagTimes_ for this variable if it's + // empty + if (last_message_times_.count(topic_name) == 0) { + last_message_times_.insert( + std::pair(topic_name, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (last_message_times_[topic_name] <= msg->header.stamp) { + RF_DEBUG( + "Update vector for " << topic_name << " is:\n" << + callback_data.update_vector_); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurement_covariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector update_vector_corrected = callback_data.update_vector_; + + // Prepare the pose data for inclusion in the filter + if (preparePose( + msg, topic_name, target_frame, pose_source_frame, callback_data.differential_, + callback_data.relative_, imu_data, update_vector_corrected, + measurement, measurement_covariance)) + { + // Store the measurement. Add a "pose" suffix so we know what kind of + // measurement we're dealing with when we debug the core filter logic. + enqueueMeasurement( + topic_name, measurement, measurement_covariance, + update_vector_corrected, + callback_data.rejection_threshold_, msg->header.stamp); + + RF_DEBUG("Enqueued new measurement for " << topic_name << "\n"); + } else { + RF_DEBUG("Did *not* enqueue measurement for " << topic_name << "\n"); + } + + last_message_times_[topic_name] = msg->header.stamp; + + RF_DEBUG( + "Last message time for " << + topic_name << " is now " << + filter_utilities::toSec(last_message_times_[topic_name]) << + "\n"); + } else { + // else if (reset_on_time_jump_ && rclcpp::Time::isSimTime()) + //{ + // reset(); + // } + + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp before that of " + "the previous message received," << " this message will be ignored. This may " + "indicate a bad timestamp. (message time: " << msg->header.stamp.nanosec << + ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + + RF_DEBUG( + "Message is too old. Last message time for " << topic_name << " is " << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << filter_utilities::toSec(msg->header.stamp) << + ".\n"); + } + + RF_DEBUG("\n----- /UkfWrapper::poseCallback (" << topic_name << ") ------\n"); +} + +void UkfWrapper::initialize() +{ + if (!parent_node_->get_clock()->started()) { + RCLCPP_INFO(parent_node_->get_logger(), "Waiting for clock to start..."); + parent_node_->get_clock()->wait_until_started(); + } + + angular_acceleration_.setZero(); + angular_acceleration_cov_.setIdentity(); + angular_acceleration_cov_ *= 1e-6; + + last_state_twist_rot_.setZero(); + + diagnostic_updater_ = std::make_unique( + parent_node_); + diagnostic_updater_->setHardwareID("none"); + + world_transform_broadcaster_ = std::make_shared( + parent_node_); + + loadParams(); + + if (print_diagnostics_) { + diagnostic_updater_->add( + "Filter diagnostic updater", this, + &UkfWrapper::aggregateDiagnostics); + } + + // Set up the frequency diagnostic + min_frequency_ = frequency_ - 2; + max_frequency_ = frequency_ + 2; + freq_diag_ = + std::make_unique( + "odometry/filtered", + *diagnostic_updater_, + diagnostic_updater::FrequencyStatusParam( + &min_frequency_, + &max_frequency_, 0.1, 10)); + + last_diag_time_ = parent_node_->now(); + last_diff_time_ = parent_node_->now().seconds(); + + // Clear out the transforms + world_base_link_trans_msg_.transform = + tf2::toMsg(tf2::Transform::getIdentity()); + + // Position publisher + rclcpp::PublisherOptions publisher_options; + publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + position_pub_ = + parent_node_->create_publisher( + "odometry/filtered", rclcpp::QoS(10), publisher_options); + + // Optional acceleration publisher + if (publish_acceleration_) { + accel_pub_ = + parent_node_->create_publisher( + "accel/filtered", rclcpp::QoS(10), publisher_options); + } + + // Block commented as it is handled in the update_rt + + // const std::chrono::duration timespan{1.0 / frequency_}; + // timer_ = rclcpp::GenericTimer::make_shared( + // parent_node_->get_clock(), std::chrono::duration_cast(timespan), + // std::bind(&UkfWrapper::periodicUpdate, this), parent_node_->get_node_base_interface()->get_context()); + // parent_node_->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void UkfWrapper::periodicUpdate() +{ + // Wait for the filter to be enabled + if (!enabled_) { + RCLCPP_INFO_ONCE( + parent_node_->get_logger(), + "Filter is disabled. To enable it call the %s service", + enable_filter_srv_->get_service_name()); + return; + } + + rclcpp::Time cur_time = parent_node_->now(); + + if (toggled_on_) { + // Now we'll integrate any measurements we've received if requested, + // and update angular acceleration. + integrateMeasurements(cur_time); + differentiateMeasurements(cur_time); + } else { + // Clear out measurements since we're not currently processing new entries + clearMeasurementQueue(); + + // Reset last measurement time so we don't get a large time delta on toggle + if (filter_.getInitializedStatus()) { + filter_.setLastMeasurementTime(parent_node_->now()); + } + } + + // Get latest state and publish it + auto filtered_position = std::make_unique(); + + bool corrected_data = false; + + if (getFilteredOdometryMessage(filtered_position.get())) { + world_base_link_trans_msg_.header.stamp = + static_cast(filtered_position->header.stamp) + tf_time_offset_; + world_base_link_trans_msg_.header.frame_id = + filtered_position->header.frame_id; + world_base_link_trans_msg_.child_frame_id = + filtered_position->child_frame_id; + + world_base_link_trans_msg_.transform.translation.x = + filtered_position->pose.pose.position.x; + world_base_link_trans_msg_.transform.translation.y = + filtered_position->pose.pose.position.y; + world_base_link_trans_msg_.transform.translation.z = + filtered_position->pose.pose.position.z; + world_base_link_trans_msg_.transform.rotation = + filtered_position->pose.pose.orientation; + + // The filtered_position is the message containing the state and covariances: + // nav_msgs Odometry + if (!validateFilterOutput(filtered_position.get())) { + RCLCPP_ERROR( + parent_node_->get_logger(), + "Critical Error, NaNs were detected in the output state of the filter. " + "This was likely due to poorly coniditioned process, noise, or sensor " + "covariances."); + } + + // If we're trying to publish with the same time stamp, it means that we had a measurement get + // inserted into the filter history, and our state estimate was updated after it was already + // published. As of ROS Noetic, TF2 will issue warnings whenever this occurs, so we make this + // behavior optional. Just for safety, we also check for the condition where the last published + // stamp is *later* than this stamp. This should never happen, but we should handle the case + // anyway. + corrected_data = (!permit_corrected_publication_ && + last_published_stamp_ >= filtered_position->header.stamp); + + // If the world_frame_id_ is the odom_frame_id_ frame, then we can just + // send the transform. If the world_frame_id_ is the map_frame_id_ frame, + // we'll have some work to do. + if (publish_transform_ && !corrected_data) { + if (filtered_position->header.frame_id == odom_frame_id_) { + world_transform_broadcaster_->sendTransform(world_base_link_trans_msg_); + } else if (filtered_position->header.frame_id == map_frame_id_) { + try { + tf2::Transform world_base_link_trans; + tf2::fromMsg( + world_base_link_trans_msg_.transform, + world_base_link_trans); + + tf2::Transform base_link_odom_trans; + tf2::fromMsg( + tf_buffer_ + ->lookupTransform( + base_link_frame_id_, + odom_frame_id_, + tf2::TimePointZero) + .transform, + base_link_odom_trans); + + /* + * First, see these two references: + * http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform + * http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction + * We have a transform from map_frame_id_->base_link_frame_id_, but + * it would actually transform a given pose from + * base_link_frame_id_->map_frame_id_. We then used lookupTransform, + * whose first two arguments are target frame and source frame, to + * get a transform from base_link_frame_id_->odom_frame_id_. + * However, this transform would actually transform data from + * odom_frame_id_->base_link_frame_id_. Now imagine that we have a + * position in the map_frame_id_ frame. First, we multiply it by the + * inverse of the map_frame_id_->baseLinkFrameId, which will + * transform that data from map_frame_id_ to base_link_frame_id_. + * Now we want to go from base_link_frame_id_->odom_frame_id_, but + * the transform we have takes data from + * odom_frame_id_->base_link_frame_id_, so we need its inverse as + * well. We have now transformed our data from map_frame_id_ to + * odom_frame_id_. However, if we want other users to be able to do + * the same, we need to broadcast the inverse of that entire + * transform. + */ + tf2::Transform map_odom_trans; + map_odom_trans.mult(world_base_link_trans, base_link_odom_trans); + + geometry_msgs::msg::TransformStamped map_odom_trans_msg; + map_odom_trans_msg.transform = tf2::toMsg(map_odom_trans); + map_odom_trans_msg.header.stamp = + static_cast(filtered_position->header.stamp) + tf_time_offset_; + map_odom_trans_msg.header.frame_id = map_frame_id_; + map_odom_trans_msg.child_frame_id = odom_frame_id_; + + world_transform_broadcaster_->sendTransform(map_odom_trans_msg); + } catch (...) { + RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE( + parent_node_->get_logger(), + *parent_node_->get_clock(), + 5000, + "Could not obtain transform from " << odom_frame_id_ << "->" << base_link_frame_id_); + } + } else { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Odometry message frame_id was " << filtered_position->header.frame_id << + ", expected " << map_frame_id_ << " or " << odom_frame_id_); + } + } + + // Retain the last published stamp so we can detect repeated transforms in future cycles + last_published_stamp_ = filtered_position->header.stamp; + + // Fire off the position and the transform + if (!corrected_data) { + position_pub_->publish(std::move(filtered_position)); + } + + if (print_diagnostics_) { + freq_diag_->tick(); + } + } + + // Publish the acceleration if desired and filter is initialized + auto filtered_acceleration = std::make_unique(); + if (!corrected_data && publish_acceleration_ && + getFilteredAccelMessage(filtered_acceleration.get())) + { + accel_pub_->publish(std::move(filtered_acceleration)); + } + + /* Diagnostics can behave strangely when playing back from bag + * files and using simulated time, so we have to check for + * time suddenly moving backwards as well as the standard + * timeout criterion before publishing. */ + + double diag_duration = (cur_time - last_diag_time_).nanoseconds(); + if (print_diagnostics_ && + (diag_duration >= diagnostic_updater_->getPeriod().nanoseconds() || + diag_duration < 0.0)) + { + diagnostic_updater_->force_update(); + last_diag_time_ = cur_time; + } + + // Clear out expired history data + if (smooth_lagged_data_) { + clearExpiredHistory(filter_.getLastMeasurementTime() - history_length_); + } + + // Warn the user if the update took too long + const double loop_elapsed = (parent_node_->now() - cur_time).seconds(); + if (loop_elapsed > 1. / frequency_) { + RCLCPP_ERROR_STREAM( + parent_node_->get_logger(), + "Failed to meet update rate! Took " << std::setprecision(20) << loop_elapsed << "seconds. " + "Try decreasing the rate, limiting sensor output frequency, or limiting the number of " + "sensors."); + } +} + +void UkfWrapper::setPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + RF_DEBUG( + "------ UkfWrapper::setPoseCallback ------\nPose message:\n" << + geometry_msgs::msg::to_yaml(*msg)); + + RCLCPP_INFO_STREAM( + parent_node_->get_logger(), + "Received set_pose request with value\n" << geometry_msgs::msg::to_yaml(*msg)); + + std::string topic_name("set_pose"); + + // Get rid of any initial poses (pretend we've never had a measurement) + initial_measurements_.clear(); + previous_measurements_.clear(); + previous_measurement_covariances_.clear(); + + clearMeasurementQueue(); + + filter_state_history_.clear(); + measurement_history_.clear(); + + // Also set the last set pose time, so we ignore all messages + // that occur before it + last_set_pose_time_ = msg->header.stamp; + + // Set the state vector to the reported pose + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + std::vector update_vector(STATE_SIZE, true); + + // We only measure pose variables, so initialize the vector to 0 + measurement.setZero(); + + // Set this to the identity and let the message reset it + measurement_covariance.setIdentity(); + measurement_covariance *= 1e-6; + + // Prepare the pose data (really just using this to transform it into the + // target frame). Twist data is going to get zeroed out. + // Since pose messages do not provide a child_frame_id, it defaults to baseLinkFrameId_ + preparePose( + msg, topic_name, world_frame_id_, base_link_frame_id_, false, false, false, + update_vector, measurement, measurement_covariance); + + // For the state + filter_.setState(measurement); + filter_.setEstimateErrorCovariance(measurement_covariance); + + filter_.setLastMeasurementTime(parent_node_->now()); + + RF_DEBUG("\n------ /UkfWrapper::setPoseCallback ------\n"); +} + +bool UkfWrapper::setPoseSrvCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr/*response*/) +{ + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg = + std::make_shared( + request->pose); + setPoseCallback(msg); + + return true; +} + +bool UkfWrapper::enableFilterSrvCallback( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr) +{ + RF_DEBUG( + "\n[" << parent_node_->get_name() << ":]" << + " ------ /UkfWrapper::enableFilterSrvCallback ------\n"); + if (enabled_) { + RCLCPP_WARN( + parent_node_->get_logger(), + "[%s:] Asking for enabling filter service, " + "but the filter was already enabled! Use param disabled_at_startup.", + parent_node_->get_name()); + } else { + RCLCPP_INFO( + parent_node_->get_logger(), + "[%s:] Enabling filter...", + parent_node_->get_name()); + enabled_ = true; + } + + return true; +} + +void UkfWrapper::twistCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const CallbackData & callback_data, const std::string & target_frame) +{ + const std::string & topic_name = callback_data.topic_name_; + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (last_set_pose_time_ >= msg->header.stamp) { + std::stringstream stream; + stream << + "The " << topic_name << + " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad " + "timestamp. (message time: " << + filter_utilities::toSec(msg->header.stamp) << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + return; + } + + RF_DEBUG( + "------ UkfWrapper::twistCallback (" << topic_name << ") ------\n" + "Twist message:\n" << geometry_msgs::msg::to_yaml(*msg)); + + if (last_message_times_.count(topic_name) == 0) { + last_message_times_.insert( + std::pair(topic_name, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (last_message_times_[topic_name] <= msg->header.stamp) { + RF_DEBUG( + "Update vector for " << topic_name << " is:\n" << + callback_data.update_vector_); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurement_covariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector update_vector_corrected = callback_data.update_vector_; + + // Prepare the twist data for inclusion in the filter + if (prepareTwist( + msg, topic_name, target_frame, update_vector_corrected, + measurement, measurement_covariance)) + { + // Store the measurement. Add a "twist" suffix so we know what kind of + // measurement we're dealing with when we debug the core filter logic. + enqueueMeasurement( + topic_name, measurement, measurement_covariance, + update_vector_corrected, + callback_data.rejection_threshold_, msg->header.stamp); + + RF_DEBUG("Enqueued new measurement for " << topic_name << "_twist\n"); + } else { + RF_DEBUG( + "Did *not* enqueue measurement for " << topic_name << + "_twist\n"); + } + + last_message_times_[topic_name] = msg->header.stamp; + + RF_DEBUG( + "Last message time for " << + topic_name << " is now " << + filter_utilities::toSec(last_message_times_[topic_name]) << + "\n"); + } else { + std::stringstream stream; + stream << "The " << topic_name << " message has a timestamp before that of " + "the previous message received," << " this message will be ignored. This may " + "indicate a bad timestamp. (message time: " << msg->header.stamp.nanosec << ")"; + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_timestamp", stream.str(), false); + + RF_DEBUG( + "Message is too old. Last message time for " << topic_name << " is" << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << filter_utilities::toSec(msg->header.stamp) << + ".\n"); + } + + RF_DEBUG("\n----- /UkfWrapper::twistCallback (" << topic_name << ") ------\n"); +} + +void UkfWrapper::addDiagnostic( + const int errLevel, + const std::string & topicAndClass, + const std::string & message, + const bool staticDiag) +{ + if (staticDiag) { + static_diagnostics_[topicAndClass] = message; + static_diag_error_level_ = std::max(static_diag_error_level_, errLevel); + } else { + dynamic_diagnostics_[topicAndClass] = message; + dynamic_diag_error_level_ = std::max(dynamic_diag_error_level_, errLevel); + } +} + +void UkfWrapper::aggregateDiagnostics( + diagnostic_updater::DiagnosticStatusWrapper & wrapper) +{ + wrapper.clear(); + wrapper.clearSummary(); + + int maxErrLevel = std::max(static_diag_error_level_, dynamic_diag_error_level_); + + // Report the overall status + switch (maxErrLevel) { + case diagnostic_msgs::msg::DiagnosticStatus::ERROR: + wrapper.summary( + maxErrLevel, + "Erroneous data or settings detected for a " + "robot_localization state estimation parent_node_->"); + break; + case + diagnostic_msgs::msg::DiagnosticStatus::WARN: wrapper.summary( + maxErrLevel, + "Potentially erroneous data or settings detected for " + "a robot_localization state estimation parent_node_->"); + break; + case diagnostic_msgs::msg::DiagnosticStatus::STALE: + wrapper.summary( + maxErrLevel, + "The state of the robot_localization state estimation " + "node is stale."); + break; + case diagnostic_msgs::msg::DiagnosticStatus::OK: + wrapper.summary( + maxErrLevel, + "The robot_localization state estimation node appears to " + "be functioning properly."); + break; + default: + break; + } + + // Aggregate all the static messages + for (std::map::iterator diagIt = + static_diagnostics_.begin(); diagIt != static_diagnostics_.end(); + ++diagIt) + { + wrapper.add(diagIt->first, diagIt->second); + } + + // Aggregate all the dynamic messages, then clear them + for (std::map::iterator diagIt = + dynamic_diagnostics_.begin(); diagIt != dynamic_diagnostics_.end(); + ++diagIt) + { + wrapper.add(diagIt->first, diagIt->second); + } + dynamic_diagnostics_.clear(); + + // Reset the warning level for the dynamic diagnostic messages + dynamic_diag_error_level_ = diagnostic_msgs::msg::DiagnosticStatus::OK; +} + +void UkfWrapper::copyCovariance( + const double * arr, Eigen::MatrixXd & covariance, + const std::string & topic_name, + const std::vector & update_vector, + const size_t offset, const size_t dimension) +{ + for (size_t i = 0; i < dimension; i++) { + for (size_t j = 0; j < dimension; j++) { + covariance(i, j) = arr[dimension * i + j]; + + if (print_diagnostics_) { + std::string iVar = state_variable_names_[offset + i]; + + if (covariance(i, j) > 1e3 && (update_vector[offset + i] || + update_vector[offset + j])) + { + std::string jVar = state_variable_names_[offset + j]; + + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << + "), which corresponds to " << (i == j ? iVar + " variance" : iVar + " and " + + jVar + " covariance") << + ", the value is extremely large (" << covariance(i, j) << "), but " + "the update vector for " << (i == j ? iVar : iVar + " and/or " + jVar) << + "is set to true. This may produce undesirable results."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_covariance", stream.str(), false); + } else if (update_vector[i] && i == j && covariance(i, j) == 0) { + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << + "), which corresponds to " << iVar << " variance, was zero. This" + "will be replaced with a small value to maintain filter stability, " + "but should be corrected at the message origin parent_node_->"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_covariance", stream.str(), false); + } else if (update_vector[i] && i == j && covariance(i, j) < 0) { + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << + "), which corresponds to " << iVar << " variance, was" + "negative. This will be replaced with a small positive value to maintain" + "filter stability, but should be corrected at the message origin parent_node_->"; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_covariance", stream.str(), false); + } + } + } + } +} + +void UkfWrapper::copyCovariance( + const Eigen::MatrixXd & covariance, double * arr, + const size_t dimension) +{ + for (size_t i = 0; i < dimension; i++) { + for (size_t j = 0; j < dimension; j++) { + arr[dimension * i + j] = covariance(i, j); + } + } +} + +std::vector UkfWrapper::loadUpdateConfig(const std::string & topic_name) +{ + std::vector update_vector(STATE_SIZE, 0); + std::string prefix = plugin_name_.empty() ? "" : plugin_name_ + "."; + const std::string topic_config_name = prefix + topic_name + "_config"; + + update_vector = parent_node_->declare_parameter(topic_config_name, update_vector); + + return update_vector; +} + +bool UkfWrapper::prepareAcceleration( + const sensor_msgs::msg::Imu::SharedPtr msg, + const std::string & topic_name, + const std::string & target_frame, + const bool relative, + std::vector & update_vector, + Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance) +{ + RF_DEBUG( + "------ UkfWrapper::prepareAcceleration (" << topic_name << + ") ------\n"); + + // 1. Get the measurement into a vector + tf2::Vector3 acc_tmp(msg->linear_acceleration.x, msg->linear_acceleration.y, + msg->linear_acceleration.z); + + // Set relevant header info + std::string msg_frame = + (msg->header.frame_id == "" ? base_link_frame_id_ : msg->header.frame_id); + + // 2. robot_localization lets users configure which variables from the sensor + // should be + // fused with the filter. This is specified at the sensor level. However, + // the data may go through transforms before being fused with the state + // estimate. In that case, we need to know which of the transformed + // variables came from the pre-transformed "approved" variables (i.e., the + // ones that had "true" in their xxx_config parameter). To do this, we + // create a pose from the original upate vector, which contains only zeros + // and ones. This pose goes through the same transforms as the measurement. + // The non-zero values that result will be used to modify the + // update_vector. + tf2::Matrix3x3 maskAcc(update_vector[StateMemberAx], 0, 0, 0, + update_vector[StateMemberAy], 0, 0, 0, + update_vector[StateMemberAz]); + + // 3. We'll need to rotate the covariance as well + Eigen::MatrixXd covariance_rotated(ACCELERATION_SIZE, ACCELERATION_SIZE); + covariance_rotated.setZero(); + + this->copyCovariance( + &(msg->linear_acceleration_covariance[0]), + covariance_rotated, topic_name, update_vector, + POSITION_A_OFFSET, ACCELERATION_SIZE); + + RF_DEBUG( + "Original measurement as tf object: " << + acc_tmp << "\nOriginal update vector:\n" << + update_vector << "\nOriginal covariance matrix:\n" << + covariance_rotated << "\n"); + + // 4. We need to transform this into the target frame (probably base_link) + // It's unlikely that we'll get a velocity measurement in another frame, but + // we have to handle the situation. + tf2::Transform target_frame_trans; + bool can_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_, + target_frame_trans); + + if (can_transform) { + const Eigen::VectorXd & state = filter_.getState(); + + // Transform to correct frame, prior to removal of gravity. + tf2::Vector3 state_twist_rot( + state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + acc_tmp = target_frame_trans.getBasis() * acc_tmp + + target_frame_trans.getOrigin().cross(angular_acceleration_) - + target_frame_trans.getOrigin().cross(state_twist_rot).cross( + state_twist_rot); + + // We don't know if the user has already handled the removal + // of normal forces, so we use a parameter + if (remove_gravitational_acceleration_[topic_name]) { + tf2::Vector3 normAcc(0, 0, gravitational_acceleration_); + tf2::Transform trans; + tf2::Vector3 rotNorm; + + if (std::abs(msg->orientation_covariance[0] + 1) < 1e-9) { + // Imu message contains no orientation, so we should use orientation + // from filter state to transform and remove acceleration + tf2::Matrix3x3 stateTmp; + stateTmp.setRPY( + state(StateMemberRoll), + state(StateMemberPitch), + state(StateMemberYaw)); + + // transform state orientation to IMU frame + trans.setBasis(stateTmp * target_frame_trans.getBasis()); + rotNorm = trans.getBasis().inverse() * normAcc; + } else { + tf2::Quaternion curAttitude; + tf2::fromMsg(msg->orientation, curAttitude); + if (std::abs(curAttitude.length() - 1.0) > 0.01) { + RCLCPP_WARN_ONCE( + parent_node_->get_logger(), + "An input was not normalized, this should NOT happen, but will normalize."); + curAttitude.normalize(); + } + trans.setRotation(curAttitude); + if (!relative) { + // curAttitude is the true world-frame attitude of the sensor + rotNorm = trans.getBasis().inverse() * normAcc; + } else { + // curAttitude is relative to the initial pose of the sensor. + // Assumption: IMU sensor is rigidly attached to the base_link + // (but a static rotation is possible). + rotNorm = target_frame_trans.getBasis().inverse() * trans.getBasis().inverse() * normAcc; + } + } + acc_tmp.setX(acc_tmp.getX() - rotNorm.getX()); + acc_tmp.setY(acc_tmp.getY() - rotNorm.getY()); + acc_tmp.setZ(acc_tmp.getZ() - rotNorm.getZ()); + + RF_DEBUG( + "Orientation is " << + trans.getRotation() << "Acceleration due to gravity is " << rotNorm << + "After removing acceleration due to gravity, acceleration is " << + acc_tmp << "\n"); + } + + maskAcc = target_frame_trans.getBasis() * maskAcc; + + // Now use the mask values to determine which update vector values should be + // true + update_vector[StateMemberAx] = static_cast( + maskAcc.getRow(StateMemberAx - POSITION_A_OFFSET).length() >= 1e-6); + update_vector[StateMemberAy] = static_cast( + maskAcc.getRow(StateMemberAy - POSITION_A_OFFSET).length() >= 1e-6); + update_vector[StateMemberAz] = static_cast( + maskAcc.getRow(StateMemberAz - POSITION_A_OFFSET).length() >= 1e-6); + + RF_DEBUG( + msg->header.frame_id << + "->" << target_frame << " transform:\n" << + target_frame_trans << "\nAfter applying transform to " << + target_frame << ", update vector is:\n" << + update_vector << "\nAfter applying transform to " << + target_frame << ", measurement is:\n" << + acc_tmp << "\n"); + + // 5. Now rotate the covariance: create an augmented + // matrix that contains a 3D rotation matrix in the + // upper-left and lower-right quadrants, and zeros + // elsewhere + tf2::Matrix3x3 rot(target_frame_trans.getRotation()); + Eigen::MatrixXd rot3d(ACCELERATION_SIZE, ACCELERATION_SIZE); + rot3d.setIdentity(); + + for (size_t r_ind = 0; r_ind < ACCELERATION_SIZE; ++r_ind) { + rot3d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot3d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot3d(r_ind, 2) = rot.getRow(r_ind).getZ(); + } + + // Carry out the rotation + covariance_rotated = rot3d * covariance_rotated.eval() * rot3d.transpose(); + + RF_DEBUG("Transformed covariance is \n" << covariance_rotated << "\n"); + + // 6. Store our corrected measurement and covariance + measurement(StateMemberAx) = acc_tmp.getX(); + measurement(StateMemberAy) = acc_tmp.getY(); + measurement(StateMemberAz) = acc_tmp.getZ(); + + // Copy the covariances + measurement_covariance.block( + POSITION_A_OFFSET, POSITION_A_OFFSET, + ACCELERATION_SIZE, ACCELERATION_SIZE) = + covariance_rotated.block(0, 0, ACCELERATION_SIZE, ACCELERATION_SIZE); + + // 7. Handle 2D mode + if (two_d_mode_) { + forceTwoD(measurement, measurement_covariance, update_vector); + } + } else { + RF_DEBUG( + "Could not transform measurement into " << target_frame << + ". Ignoring...\n"); + } + + RF_DEBUG( + "\n----- /UkfWrapper::prepareAcceleration(" << topic_name << + ") ------\n"); + + return can_transform; +} + +bool UkfWrapper::preparePose( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, + const std::string & topic_name, const std::string & target_frame, + const std::string & source_frame, + const bool differential, const bool relative, const bool imu_data, + std::vector & update_vector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance) +{ + bool retVal = false; + + RF_DEBUG("------ UkfWrapper::preparePose (" << topic_name << ") ------\n"); + + // 1. Get the measurement into a tf-friendly transform (pose) object + tf2::Stamped pose_tmp; + + // We'll need this later for storing this measurement for differential + // integration + tf2::Transform cur_measurement; + + // Handle issues where frame_id data is not filled out properly + // @todo: verify that this is necessary still. New IMU handling may + // have rendered this obsolete. + std::string final_target_frame; + if (target_frame == "") { + if (msg->header.frame_id == "") { + // Blank target and message frames mean we can just + // use our world_frame + final_target_frame = world_frame_id_; + pose_tmp.frame_id_ = final_target_frame; + } else { + // Under the (target_frame == "") condition, + // a blank target frame means we shouldn't bother + // transforming the data + final_target_frame = msg->header.frame_id; + pose_tmp.frame_id_ = final_target_frame; + } + } else { + // Otherwise, we should use our target frame + final_target_frame = target_frame; + pose_tmp.frame_id_ = + (differential && !imu_data ? final_target_frame : msg->header.frame_id); + } + + RF_DEBUG( + "Final target frame for " << topic_name << " is " << + final_target_frame << "\n"); + + pose_tmp.stamp_ = tf2::timeFromSec( + static_cast(msg->header.stamp.sec) + + static_cast(msg->header.stamp.nanosec) / 1000000000.0); + + // Fill out the position data + pose_tmp.setOrigin( + tf2::Vector3( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z)); + + tf2::Quaternion orientation; + + // Handle bad (empty) quaternions + if (msg->pose.pose.orientation.x == 0 && msg->pose.pose.orientation.y == 0 && + msg->pose.pose.orientation.z == 0 && msg->pose.pose.orientation.w == 0) + { + orientation.setValue(0.0, 0.0, 0.0, 1.0); + + if (update_vector[StateMemberRoll] || + update_vector[StateMemberPitch] || + update_vector[StateMemberYaw]) + { + std::stringstream stream; + stream << "The " << topic_name << + " message contains an invalid orientation quaternion, " << + "but its configuration is such that orientation data is being used." + " Correcting..."; + + addDiagnostic( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + topic_name + "_orientation", stream.str(), false); + } + } else { + tf2::fromMsg(msg->pose.pose.orientation, orientation); + if (std::abs(orientation.length() - 1.0) > 0.01) { + RCLCPP_WARN_ONCE( + parent_node_->get_logger(), + "An input was not normalized, this should NOT happen, but will normalize."); + orientation.normalize(); + } + } + + // Fill out the orientation data + pose_tmp.setRotation(orientation); + + // 2. Get the target frame transformation + tf2::Transform target_frame_trans; + bool can_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_.get(), final_target_frame, pose_tmp.frame_id_, + rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, + target_frame_trans); + + // handling multiple odometry origins: convert to the origin adherent to base_link. + // make pose refer to the baseLinkFrame as source + tf2::Transform source_frame_trans; + bool can_src_transform = false; + if (source_frame != base_link_frame_id_) { + can_src_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_.get(), source_frame, base_link_frame_id_, + rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_, + source_frame_trans); + } + + // 3. Make sure we can work with this data before carrying on + if (can_transform) { + /* 4. robot_localization lets users configure which variables from the + * sensor should be fused with the filter. This is specified at the sensor + * level. However, the data may go through transforms before being fused + * with the state estimate. In that case, we need to know which of the + * transformed variables came from the pre-transformed "approved" variables + * (i.e., the ones that had "true" in their xxx_config parameter). To do + * this, we construct matrices using the update vector values on the + * diagonals, pass this matrix through the rotation, and use the length of + * each row to determine the transformed update vector. The process is + * slightly different for IMUs, as the coordinate frame transform is really + * the base_link->imu_frame transform, and not a transform from some other + * world-fixed frame (even though the IMU data itself *is* reported in a + * world fixed frame). */ + tf2::Matrix3x3 mask_position(update_vector[StateMemberX], 0, 0, 0, + update_vector[StateMemberY], 0, 0, 0, + update_vector[StateMemberZ]); + + tf2::Matrix3x3 mask_orientation(update_vector[StateMemberRoll], 0, 0, 0, + update_vector[StateMemberPitch], 0, 0, 0, + update_vector[StateMemberYaw]); + + if (imu_data) { + /* We have to treat IMU orientation data differently. Even though we are + * dealing with pose data when we work with orientations, for IMUs, the + * frame_id is the frame in which the sensor is mounted, and not the + * coordinate frame of the IMU. Imagine an IMU that is mounted facing + * sideways. The pitch in the IMU frame becomes roll for the vehicle. This + * means that we need to rotate roll and pitch angles by the IMU's + * mounting yaw offset, and we must apply similar treatment to its update + * mask and covariance. + * */ + + double dummy, yaw; + target_frame_trans.getBasis().getRPY(dummy, dummy, yaw); + tf2::Matrix3x3 trans_tmp; + trans_tmp.setRPY(0.0, 0.0, yaw); + + mask_position = trans_tmp * mask_position; + mask_orientation = trans_tmp * mask_orientation; + } else { + mask_position = target_frame_trans.getBasis() * mask_position; + mask_orientation = target_frame_trans.getBasis() * mask_orientation; + } + + // Now copy the mask values back into the update vector: any row with a + // significant vector length indicates that we want to set that variable to + // true in the update vector. + update_vector[StateMemberX] = static_cast( + mask_position.getRow(StateMemberX - POSITION_OFFSET).length() >= 1e-6); + update_vector[StateMemberY] = static_cast( + mask_position.getRow(StateMemberY - POSITION_OFFSET).length() >= 1e-6); + update_vector[StateMemberZ] = static_cast( + mask_position.getRow(StateMemberZ - POSITION_OFFSET).length() >= 1e-6); + update_vector[StateMemberRoll] = static_cast( + mask_orientation.getRow(StateMemberRoll - ORIENTATION_OFFSET) + .length() >= 1e-6); + update_vector[StateMemberPitch] = static_cast( + mask_orientation.getRow(StateMemberPitch - ORIENTATION_OFFSET) + .length() >= 1e-6); + update_vector[StateMemberYaw] = static_cast( + mask_orientation.getRow(StateMemberYaw - ORIENTATION_OFFSET).length() >= + 1e-6); + + // 5a. We'll need to rotate the covariance as well. Create a container and + // copy over the covariance data + Eigen::MatrixXd covariance(POSE_SIZE, POSE_SIZE); + covariance.setZero(); + copyCovariance( + &(msg->pose.covariance[0]), covariance, topic_name, + update_vector, POSITION_OFFSET, POSE_SIZE); + + // 5b. Now rotate the covariance: create an augmented matrix that + // contains a 3D rotation matrix in the upper-left and lower-right + // quadrants, with zeros elsewhere. + tf2::Matrix3x3 rot; + Eigen::MatrixXd rot6d(POSE_SIZE, POSE_SIZE); + rot6d.setIdentity(); + Eigen::MatrixXd covariance_rotated; + + // Transform pose covariance due to a different pose source origin + if (can_src_transform) { + // (source_frame != base_link_frame_id_) already satisfied + rot.setRotation(source_frame_trans.getRotation()); + for (size_t r_ind = 0; r_ind < POSITION_SIZE; ++r_ind) { + // let's borrow rot6d here... + rot6d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot6d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot6d(r_ind, 2) = rot.getRow(r_ind).getZ(); + rot6d(r_ind + POSITION_SIZE, 3) = rot.getRow(r_ind).getX(); + rot6d(r_ind + POSITION_SIZE, 4) = rot.getRow(r_ind).getY(); + rot6d(r_ind + POSITION_SIZE, 5) = rot.getRow(r_ind).getZ(); + } + // since the transformation is a post-multiply + covariance = rot6d.transpose() * covariance.eval() * rot6d; + } + // return rot6d to its initial state. + rot6d.setIdentity(); + + if (imu_data) { + // Apply the same special logic to the IMU covariance rotation + double dummy, yaw; + target_frame_trans.getBasis().getRPY(dummy, dummy, yaw); + rot.setRPY(0.0, 0.0, yaw); + } else { + rot.setRotation(target_frame_trans.getRotation()); + } + + for (size_t r_ind = 0; r_ind < POSITION_SIZE; ++r_ind) { + rot6d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot6d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot6d(r_ind, 2) = rot.getRow(r_ind).getZ(); + rot6d(r_ind + POSITION_SIZE, 3) = rot.getRow(r_ind).getX(); + rot6d(r_ind + POSITION_SIZE, 4) = rot.getRow(r_ind).getY(); + rot6d(r_ind + POSITION_SIZE, 5) = rot.getRow(r_ind).getZ(); + } + + // Now carry out the rotation + covariance_rotated = rot6d * covariance * rot6d.transpose(); + + RF_DEBUG( + "After rotating into the " << final_target_frame << + " frame, covariance is \n" << + covariance_rotated << "\n"); + + /* 6a. For IMU data, the transform that we get is the transform from the + * body frame of the robot (e.g., base_link) to the mounting frame of the + * robot. It is *not* the coordinate frame in which the IMU orientation data + * is reported. If the IMU is mounted in a non-neutral orientation, we need + * to remove those offsets, and then we need to potentially "swap" roll and + * pitch. Note that this transform does NOT handle NED->ENU conversions. + * Data is assumed to be in the ENU frame when it is received. + * */ + if (imu_data) { + // First, convert the transform and measurement rotation to RPY + // @todo: There must be a way to handle this with quaternions. Need to + // look into it. + double roll_offset = 0; + double pitch_offset = 0; + double yaw_offset = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + ros_filter_utilities::quatToRPY( + target_frame_trans.getRotation(), + roll_offset, pitch_offset, yaw_offset); + ros_filter_utilities::quatToRPY(pose_tmp.getRotation(), roll, pitch, yaw); + + // 6b. Apply the offset (making sure to bound them), and throw them in a + // vector + tf2::Vector3 rpy_angles( + angles::normalize_angle(roll - roll_offset), + angles::normalize_angle(pitch - pitch_offset), + angles::normalize_angle(yaw - yaw_offset)); + + // 6c. Now we need to rotate the roll and pitch by the yaw offset value. + // Imagine a case where an IMU is mounted facing sideways. In that case + // pitch for the IMU's world frame is roll for the robot. + tf2::Matrix3x3 mat; + mat.setRPY(0.0, 0.0, yaw_offset); + rpy_angles = mat * rpy_angles; + pose_tmp.getBasis().setRPY( + rpy_angles.getX(), rpy_angles.getY(), + rpy_angles.getZ()); + + // We will use this target transformation later on, but + // we've already transformed this data as if the IMU + // were mounted neutrall on the robot, so we can just + // make the transform the identity. + target_frame_trans.setIdentity(); + } + + // 7. Two cases: if we're in differential mode, we need to generate a twist + // message. Otherwise, we just transform it to the target frame. + if (differential) { + bool success = false; + + // We're going to be playing with pose_tmp, so store it, + // as we'll need to save its current value for the next + // measurement. + cur_measurement = pose_tmp; + + // Make sure we have previous measurements to work with + if (previous_measurements_.count(topic_name) > 0 && + previous_measurement_covariances_.count(topic_name) > 0) + { + // 7a. If we are carrying out differential integration and + // we have a previous measurement for this sensor,then we + // need to apply the inverse of that measurement to this new + // measurement to produce a "delta" measurement between the two. + // Even if we're not using all of the variables from this sensor, + // we need to use the whole measurement to determine the delta + // to the new measurement + tf2::Transform prev_measurement = previous_measurements_[topic_name]; + pose_tmp.setData(prev_measurement.inverseTimes(pose_tmp)); + + RF_DEBUG( + "Previous measurement:\n" << + previous_measurements_[topic_name] << + "\nAfter removing previous measurement, measurement delta is:\n" << + pose_tmp << "\n"); + + // 7b. Now we we have a measurement delta in the frame_id of the + // message, but we want that delta to be in the target frame, so + // we need to apply the rotation of the target frame transform. + target_frame_trans.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + pose_tmp.mult(target_frame_trans, pose_tmp); + + RF_DEBUG( + "After rotating to the target frame, measurement delta is:\n" << + pose_tmp << "\n"); + + // 7c. Now use the time difference from the last message to compute + // translational and rotational velocities + double dt = filter_utilities::toSec(msg->header.stamp) - + filter_utilities::toSec(last_message_times_[topic_name]); + double xVel = pose_tmp.getOrigin().getX() / dt; + double yVel = pose_tmp.getOrigin().getY() / dt; + double zVel = pose_tmp.getOrigin().getZ() / dt; + + double rollVel = 0; + double pitchVel = 0; + double yawVel = 0; + + ros_filter_utilities::quatToRPY( + pose_tmp.getRotation(), rollVel, + pitchVel, yawVel); + rollVel /= dt; + pitchVel /= dt; + yawVel /= dt; + + RF_DEBUG( + "Previous message time was " << + filter_utilities::toSec(last_message_times_[topic_name]) << + ", current message time is " << + filter_utilities::toSec(msg->header.stamp) << ", delta is " << + dt << ", velocity is (vX, vY, vZ): (" << xVel << ", " << + yVel << ", " << zVel << ")\n" << + "(vRoll, vPitch, vYaw): (" << rollVel << ", " << pitchVel << + ", " << yawVel << ")\n"); + + // 7d. Fill out the velocity data in the message + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist_ptr = + std::make_shared(); + twist_ptr->header = msg->header; + twist_ptr->header.frame_id = source_frame; + twist_ptr->twist.twist.linear.x = xVel; + twist_ptr->twist.twist.linear.y = yVel; + twist_ptr->twist.twist.linear.z = zVel; + twist_ptr->twist.twist.angular.x = rollVel; + twist_ptr->twist.twist.angular.y = pitchVel; + twist_ptr->twist.twist.angular.z = yawVel; + std::vector twist_update_vec(STATE_SIZE, false); + std::copy( + update_vector.begin() + POSITION_OFFSET, + update_vector.begin() + POSE_SIZE, + twist_update_vec.begin() + POSITION_V_OFFSET); + std::copy( + twist_update_vec.begin(), twist_update_vec.end(), + update_vector.begin()); + + // 7e. Now rotate the previous covariance for this measurement to get it + // into the target frame, and add the current measurement's rotated + // covariance to the previous measurement's rotated covariance, and + // multiply by the time delta. + Eigen::MatrixXd prev_covar_rotated = + rot6d * previous_measurement_covariances_[topic_name] * + rot6d.transpose(); + covariance_rotated = + (covariance_rotated.eval() + prev_covar_rotated) * dt; + copyCovariance( + covariance_rotated, &(twist_ptr->twist.covariance[0]), + POSE_SIZE); + + RF_DEBUG( + "Previous measurement covariance:\n" << + previous_measurement_covariances_[topic_name] << + "\nPrevious measurement covariance rotated:\n" << + prev_covar_rotated << "\nFinal twist covariance:\n" << + covariance_rotated << "\n"); + + // Now pass this on to prepareTwist, which will convert it to the + // required frame + success = prepareTwist( + twist_ptr, topic_name + "_twist", + base_link_frame_id_, update_vector, + measurement, measurement_covariance); + } + + // 7f. Update the previous measurement and measurement covariance + previous_measurements_[topic_name] = cur_measurement; + previous_measurement_covariances_[topic_name] = covariance; + + retVal = success; + } else { + // make pose refer to the baseLinkFrame as source + // can_src_transform == true => ( sourceFrame != baseLinkFrameId_ ) + if (can_src_transform) { + pose_tmp.setData(pose_tmp * source_frame_trans); + } + + // 7g. If we're in relative mode, remove the initial measurement + if (relative) { + if (initial_measurements_.count(topic_name) == 0) { + initial_measurements_.insert( + std::pair(topic_name, pose_tmp)); + } + + tf2::Transform initial_measurement = initial_measurements_[topic_name]; + pose_tmp.setData(initial_measurement.inverseTimes(pose_tmp)); + } + + // 7h. Apply the target frame transformation to the pose object. + pose_tmp.mult(target_frame_trans, pose_tmp); + pose_tmp.frame_id_ = final_target_frame; + + // 7i. Finally, copy everything into our measurement and covariance + // objects + measurement(StateMemberX) = pose_tmp.getOrigin().x(); + measurement(StateMemberY) = pose_tmp.getOrigin().y(); + measurement(StateMemberZ) = pose_tmp.getOrigin().z(); + + // The filter needs roll, pitch, and yaw values instead of quaternions + double roll, pitch, yaw; + ros_filter_utilities::quatToRPY(pose_tmp.getRotation(), roll, pitch, yaw); + measurement(StateMemberRoll) = roll; + measurement(StateMemberPitch) = pitch; + measurement(StateMemberYaw) = yaw; + + measurement_covariance.block(0, 0, POSE_SIZE, POSE_SIZE) = + covariance_rotated.block(0, 0, POSE_SIZE, POSE_SIZE); + + // 8. Handle 2D mode + if (two_d_mode_) { + forceTwoD(measurement, measurement_covariance, update_vector); + } + + retVal = true; + } + } else { + retVal = false; + + RF_DEBUG( + "Could not transform measurement into " << final_target_frame << + ". Ignoring..."); + } + + RF_DEBUG("\n----- /UkfWrapper::preparePose (" << topic_name << ") ------\n"); + + return retVal; +} + +bool UkfWrapper::prepareTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg, + const std::string & topic_name, const std::string & target_frame, + std::vector & update_vector, Eigen::VectorXd & measurement, + Eigen::MatrixXd & measurement_covariance) +{ + RF_DEBUG("------ UkfWrapper::prepareTwist (" << topic_name << ") ------\n"); + + // 1. Get the measurement into two separate vector objects. + tf2::Vector3 twist_lin(msg->twist.twist.linear.x, msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + tf2::Vector3 meas_twist_rot(msg->twist.twist.angular.x, + msg->twist.twist.angular.y, + msg->twist.twist.angular.z); + + // 1a. This sensor may or may not measure rotational velocity. Regardless, + // if it measures linear velocity, then later on, we'll need to remove "false" + // linear velocity resulting from angular velocity and the translational + // offset of the sensor from the vehicle origin. + const Eigen::VectorXd & state = filter_.getState(); + tf2::Vector3 state_twist_rot(state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + + // Determine the frame_id of the data + std::string msg_frame = + (msg->header.frame_id == "" ? target_frame : msg->header.frame_id); + + // 2. robot_localization lets users configure which variables from the sensor + // should be + // fused with the filter. This is specified at the sensor level. However, + // the data may go through transforms before being fused with the state + // estimate. In that case, we need to know which of the transformed + // variables came from the pre-transformed "approved" variables (i.e., the + // ones that had "true" in their xxx_config parameter). To do this, we + // construct matrices using the update vector values on the diagonals, pass + // this matrix through the rotation, and use the length of each row to + // determine the transformed update vector. + tf2::Matrix3x3 maskLin(update_vector[StateMemberVx], 0, 0, 0, + update_vector[StateMemberVy], 0, 0, 0, + update_vector[StateMemberVz]); + + tf2::Matrix3x3 maskRot(update_vector[StateMemberVroll], 0, 0, 0, + update_vector[StateMemberVpitch], 0, 0, 0, + update_vector[StateMemberVyaw]); + + // 3. We'll need to rotate the covariance as well + Eigen::MatrixXd covariance_rotated(TWIST_SIZE, TWIST_SIZE); + covariance_rotated.setZero(); + + copyCovariance( + &(msg->twist.covariance[0]), covariance_rotated, topic_name, + update_vector, POSITION_V_OFFSET, TWIST_SIZE); + + RF_DEBUG( + "Original measurement as tf object:\nLinear: " << + twist_lin << "Rotational: " << meas_twist_rot << + "\nOriginal update vector:\n" << + update_vector << "\nOriginal covariance matrix:\n" << + covariance_rotated << "\n"); + + // 4. We need to transform this into the target frame (probably base_link) + tf2::Transform target_frame_trans; + bool can_transform = ros_filter_utilities::lookupTransformSafe( + tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_, + target_frame_trans); + + if (can_transform) { + // Transform to correct frame. Note that we can get linear velocity + // as a result of the sensor offset and rotational velocity + meas_twist_rot = target_frame_trans.getBasis() * meas_twist_rot; + twist_lin = target_frame_trans.getBasis() * twist_lin + + target_frame_trans.getOrigin().cross(state_twist_rot); + maskLin = target_frame_trans.getBasis() * maskLin; + maskRot = target_frame_trans.getBasis() * maskRot; + + // Now copy the mask values back into the update vector + update_vector[StateMemberVx] = static_cast( + maskLin.getRow(StateMemberVx - POSITION_V_OFFSET).length() >= 1e-6); + update_vector[StateMemberVy] = static_cast( + maskLin.getRow(StateMemberVy - POSITION_V_OFFSET).length() >= 1e-6); + update_vector[StateMemberVz] = static_cast( + maskLin.getRow(StateMemberVz - POSITION_V_OFFSET).length() >= 1e-6); + update_vector[StateMemberVroll] = static_cast( + maskRot.getRow(StateMemberVroll - ORIENTATION_V_OFFSET).length() >= + 1e-6); + update_vector[StateMemberVpitch] = static_cast( + maskRot.getRow(StateMemberVpitch - ORIENTATION_V_OFFSET).length() >= + 1e-6); + update_vector[StateMemberVyaw] = static_cast( + maskRot.getRow(StateMemberVyaw - ORIENTATION_V_OFFSET).length() >= + 1e-6); + + RF_DEBUG( + msg->header.frame_id << + "->" << target_frame << " transform:\n" << + target_frame_trans << "\nAfter applying transform to " << + target_frame << ", update vector is:\n" << + update_vector << "\nAfter applying transform to " << + target_frame << ", measurement is:\n" << + "Linear: " << twist_lin << "Rotational: " << meas_twist_rot << + "\n"); + + // 5. Now rotate the covariance: create an augmented + // matrix that contains a 3D rotation matrix in the + // upper-left and lower-right quadrants, and zeros + // elsewhere + tf2::Matrix3x3 rot(target_frame_trans.getRotation()); + Eigen::MatrixXd rot6d(TWIST_SIZE, TWIST_SIZE); + rot6d.setIdentity(); + + for (size_t r_ind = 0; r_ind < POSITION_SIZE; ++r_ind) { + rot6d(r_ind, 0) = rot.getRow(r_ind).getX(); + rot6d(r_ind, 1) = rot.getRow(r_ind).getY(); + rot6d(r_ind, 2) = rot.getRow(r_ind).getZ(); + rot6d(r_ind + POSITION_SIZE, 3) = rot.getRow(r_ind).getX(); + rot6d(r_ind + POSITION_SIZE, 4) = rot.getRow(r_ind).getY(); + rot6d(r_ind + POSITION_SIZE, 5) = rot.getRow(r_ind).getZ(); + } + + // Carry out the rotation + covariance_rotated = rot6d * covariance_rotated.eval() * rot6d.transpose(); + + RF_DEBUG("Transformed covariance is \n" << covariance_rotated << "\n"); + + // 6. Store our corrected measurement and covariance + measurement(StateMemberVx) = twist_lin.getX(); + measurement(StateMemberVy) = twist_lin.getY(); + measurement(StateMemberVz) = twist_lin.getZ(); + measurement(StateMemberVroll) = meas_twist_rot.getX(); + measurement(StateMemberVpitch) = meas_twist_rot.getY(); + measurement(StateMemberVyaw) = meas_twist_rot.getZ(); + + // Copy the covariances + measurement_covariance.block( + POSITION_V_OFFSET, POSITION_V_OFFSET, + TWIST_SIZE, TWIST_SIZE) = + covariance_rotated.block(0, 0, TWIST_SIZE, TWIST_SIZE); + + // 7. Handle 2D mode + if (two_d_mode_) { + forceTwoD(measurement, measurement_covariance, update_vector); + } + } else { + RF_DEBUG( + "Could not transform measurement into " << target_frame << + ". Ignoring..."); + } + + RF_DEBUG("\n----- /UkfWrapper::prepareTwist (" << topic_name << ") ------\n"); + + return can_transform; +} + +void UkfWrapper::saveFilterState(robot_localization::Ukf & filter) +{ + FilterStatePtr state = FilterStatePtr(new FilterState()); + state->state_ = Eigen::VectorXd(filter.getState()); + state->estimate_error_covariance_ = + Eigen::MatrixXd(filter.getEstimateErrorCovariance()); + state->last_measurement_time_ = filter.getLastMeasurementTime(); + state->latest_control_ = Eigen::VectorXd(filter.getControl()); + state->latest_control_time_ = filter.getControlTime(); + filter_state_history_.push_back(state); + RF_DEBUG( + "Saved state with timestamp " << + std::setprecision(20) << + filter_utilities::toSec(state->last_measurement_time_) << + " to history. " << filter_state_history_.size() << + " measurements are in the queue.\n"); +} + +bool UkfWrapper::revertTo(const rclcpp::Time & time) +{ + RF_DEBUG("\n----- UkfWrapper::revertTo -----\n"); + RF_DEBUG( + "\nRequested time was " << std::setprecision(20) << + filter_utilities::toSec(time) << "\n") + + // size_t history_size = filter_state_history_.size(); + + // Walk back through the queue until we reach a filter state whose time stamp + // is less than or equal to the requested time. Since every saved state after + // that time will be overwritten/corrected, we can pop from the queue. If the + // history is insufficiently short, we just take the oldest state we have. + FilterStatePtr last_history_state; + while (!filter_state_history_.empty() && + filter_state_history_.back()->last_measurement_time_ > time) + { + last_history_state = filter_state_history_.back(); + filter_state_history_.pop_back(); + } + + // If the state history is not empty at this point, it means that our history + // was large enough, and we should revert to the state at the back of the + // history deque. + bool ret_val = false; + if (!filter_state_history_.empty()) { + ret_val = true; + last_history_state = filter_state_history_.back(); + } else { + RF_DEBUG( + "Insufficient history to revert to time " << + filter_utilities::toSec(time) << "\n"); + + if (last_history_state) { + RF_DEBUG( + "Will revert to oldest state at " << + filter_utilities::toSec(last_history_state->latest_control_time_) << + ".\n"); + + // ROS_WARN_STREAM_DELAYED_THROTTLE(history_length_, "Could not revert " + // "to state with time " << std::setprecision(20) << time << + // ". Instead reverted to state with time " << + // lastHistoryState->lastMeasurementTime_ << ". History size was " << + // history_size); + } + } + + // If we have a valid reversion state, revert + if (last_history_state) { + // Reset filter to the latest state from the queue. + const FilterStatePtr & state = last_history_state; + filter_.setState(state->state_); + filter_.setEstimateErrorCovariance(state->estimate_error_covariance_); + filter_.setLastMeasurementTime(state->last_measurement_time_); + + RF_DEBUG( + "Reverted to state with time " << + filter_utilities::toSec(state->last_measurement_time_) << "\n"); + + // Repeat for measurements, but push every measurement onto the measurement + // queue as we go + int restored_measurements = 0; + while (!measurement_history_.empty() && + measurement_history_.back()->time_ > time) + { + // Don't need to restore measurements that predate our earliest state time + if (state->last_measurement_time_ <= measurement_history_.back()->time_) { + measurement_queue_.push(measurement_history_.back()); + restored_measurements++; + } + + measurement_history_.pop_back(); + } + + RF_DEBUG( + "Restored " << restored_measurements << " to measurement queue." + "\n"); + } + + RF_DEBUG("\n----- /UkfWrapper::revertTo\n"); + + return ret_val; +} + +bool UkfWrapper::validateFilterOutput(nav_msgs::msg::Odometry * message) +{ + return !std::isnan(message->pose.pose.position.x) && + !std::isinf(message->pose.pose.position.x) && + !std::isnan(message->pose.pose.position.y) && + !std::isinf(message->pose.pose.position.y) && + !std::isnan(message->pose.pose.position.z) && + !std::isinf(message->pose.pose.position.z) && + !std::isnan(message->pose.pose.orientation.x) && + !std::isinf(message->pose.pose.orientation.x) && + !std::isnan(message->pose.pose.orientation.y) && + !std::isinf(message->pose.pose.orientation.y) && + !std::isnan(message->pose.pose.orientation.z) && + !std::isinf(message->pose.pose.orientation.z) && + !std::isnan(message->pose.pose.orientation.w) && + !std::isinf(message->pose.pose.orientation.w) && + !std::isnan(message->twist.twist.linear.x) && + !std::isinf(message->twist.twist.linear.x) && + !std::isnan(message->twist.twist.linear.y) && + !std::isinf(message->twist.twist.linear.y) && + !std::isnan(message->twist.twist.linear.z) && + !std::isinf(message->twist.twist.linear.z) && + !std::isnan(message->twist.twist.angular.x) && + !std::isinf(message->twist.twist.angular.x) && + !std::isnan(message->twist.twist.angular.y) && + !std::isinf(message->twist.twist.angular.y) && + !std::isnan(message->twist.twist.angular.z) && + !std::isinf(message->twist.twist.angular.z); +} + +void UkfWrapper::clearExpiredHistory(const rclcpp::Time cutoff_time) +{ + RF_DEBUG( + "\n----- UkfWrapper::clearExpiredHistory -----" << + "\nCutoff time is " << filter_utilities::toSec(cutoff_time) << + "\n"); + + int popped_measurements = 0; + int popped_states = 0; + + while (!measurement_history_.empty() && + measurement_history_.front()->time_ < cutoff_time) + { + measurement_history_.pop_front(); + popped_measurements++; + } + + while (!filter_state_history_.empty() && + filter_state_history_.front()->last_measurement_time_ < cutoff_time) + { + filter_state_history_.pop_front(); + popped_states++; + } + + RF_DEBUG( + "\nPopped " << popped_measurements << " measurements and " << + popped_states << + " states from their respective queues." << + "\n---- /UkfWrapper::clearExpiredHistory ----\n"); +} + +void UkfWrapper::clearMeasurementQueue() +{ + // Clear the measurement queue. + // This prevents us from immediately undoing our reset. + while (!measurement_queue_.empty() && rclcpp::ok()) { + measurement_queue_.pop(); + } +} +} // namespace robot_localization diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt index 6c3e2b6c..f18114bc 100644 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ b/localizers/easynav_gps_localizer/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index a989cfa3..4c45be17 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -1,21 +1,25 @@ # easynav_gps_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_gps_localizer/GpsLocalizer` - **Type:** `easynav::GpsLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,33 +27,36 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- - **Description:** GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. ## Parameters + This plugin does not declare configurable ROS parameters. ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `robot/gps/fix` | `sensor_msgs/msg/NavSatFix` | Raw GPS fix. | SensorDataQoS (reliable) | | Subscription | `imu/data` | `sensor_msgs/msg/Imu` | IMU orientation for yaw fusion. | SensorDataQoS (reliable) | | Publisher | `robot/odom_gps` | `nav_msgs/msg/Odometry` | Odometry fused from GPS + IMU (UTM-projected). | SensorDataQoS | - ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | GPS-based odometry estimate. | - ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map (or world) -> odom_gps` | Static or slowly varying transform aligning UTM to local odometry frame. | | Optional | `base_link -> imu` | Used implicitly via IMU orientation; not looked up explicitly in this plugin. | - ## License + GPL-3.0-only diff --git a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp index a6ece830..cf0192d6 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -23,16 +23,11 @@ #ifndef EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ #define EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ -#include - #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "easynav_core/LocalizerMethodBase.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_ros/static_transform_broadcaster.hpp" #include "sensor_msgs/msg/imu.hpp" -#include "tf2/LinearMath/Quaternion.hpp" -#include "tf2/LinearMath/Matrix3x3.hpp" #include namespace easynav @@ -64,9 +59,9 @@ class GpsLocalizer : public easynav::LocalizerMethodBase * * This override may be used to set up internal resources. By default, it simply succeeds. * - * @return std::expected Returns success or an error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the localization estimate based on the current navigation state. diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index a50a7161..3a852fd9 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -20,7 +20,6 @@ /// \file /// \brief Implementation of the GpsLocalizer class. -#include #include "easynav_gps_localizer/GpsLocalizer.hpp" #include "easynav_common/RTTFBuffer.hpp" @@ -28,14 +27,15 @@ namespace easynav { -std::expected GpsLocalizer::on_initialize() +void GpsLocalizer::on_initialize() { auto node = get_node(); // Initialize the odometry message odom_.header.stamp = get_node()->now(); - odom_.header.frame_id = get_tf_prefix() + "map"; - odom_.child_frame_id = get_tf_prefix() + "base_link"; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_.header.frame_id = tf_info.map_frame; + odom_.child_frame_id = tf_info.robot_frame; // Create subscriber to GPS data gps_subscriber_ = node->create_subscription( @@ -57,8 +57,8 @@ std::expected GpsLocalizer::on_initialize() // Create static transform geometry_msgs::msg::TransformStamped transform; transform.header.stamp = node->now(); - transform.header.frame_id = get_tf_prefix() + "map"; - transform.child_frame_id = get_tf_prefix() + "odom"; + transform.header.frame_id = tf_info.map_frame; + transform.child_frame_id = tf_info.odom_frame; transform.transform.translation.x = 0.0; transform.transform.translation.y = 0.0; transform.transform.translation.z = 0.0; @@ -72,8 +72,6 @@ std::expected GpsLocalizer::on_initialize() time_1_ = get_node()->now().seconds(); alpha_ = 0.99; - - return {}; } void GpsLocalizer::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) @@ -114,8 +112,9 @@ void GpsLocalizer::update(NavState & nav_state) // Get XY cartesian coordinates respect to the origin odom_.header.stamp = gps_msg_.header.stamp; - odom_.header.frame_id = get_tf_prefix() + "map"; - odom_.child_frame_id = get_tf_prefix() + "base_link"; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_.header.frame_id = tf_info.map_frame; + odom_.child_frame_id = tf_info.robot_frame; odom_.pose.pose.position.x = utm_x - origin_utm_.x; odom_.pose.pose.position.y = utm_y - origin_utm_.y; diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index b55471df..12e237f4 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(navmap_core REQUIRED) diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index 93ce6657..e1451c27 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -1,21 +1,25 @@ # easynav_navmap_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Easy Navigation: nAVmAP Localizer package. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_navmap_localizer/AMCLLocalizer` - **Type:** `easynav::navmap::AMCLLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,6 +27,7 @@ Easy Navigation: nAVmAP Localizer package. - **Description:** A default "navmap" implementation for the AMCL localizer. ## Parameters + All parameters are declared under the plugin name namespace, i.e., `//easynav_navmap_localizer/AMCLLocalizer/...`. | Name | Type | Default | Description | @@ -50,16 +55,19 @@ All parameters are declared under the plugin name namespace, i.e., `// ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Subscription | `/odom` | `nav_msgs/msg/Odometry` | Used only when .compute_odom_from_tf = false | SensorDataQoS (reliable) | +| Subscription | `/odom` | `nav_msgs/msg/Odometry` | Used only when `.compute_odom_from_tf = false` | SensorDataQoS (reliable) | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Publishes the estimated pose with covariance | depth=10 | ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `imu` | `IMUPerceptions` | **Read** | Used to get the latest IMU orientation when available. | @@ -70,11 +78,13 @@ This package does not create service servers or clients. | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Estimated robot pose stored at the end of update/predict. | ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map -> odom` | Broadcasts the global transform that aligns the odometry frame with the map frame. | -| Requires | `odom -> base_footprint` | Required only when .compute_odom_from_tf = true (read from TF). | +| Requires | `odom -> base_footprint` | Required only when `.compute_odom_from_tf = true` (read from TF). | | Requires | `base_footprint -> ` | Required to transform perception point clouds into the robot base frame during correction. | ## License + GPL-3.0-only diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index fbfd32a9..23d2c45f 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -24,13 +24,9 @@ #define EASYNAV_NAVMAP_LOCALIZER__AMCLLOCALIZER_HPP_ #include -#include -#include -#include -#include -#include #include #include +#include #include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" @@ -79,9 +75,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -188,7 +184,7 @@ class AMCLLocalizer : public LocalizerMethodBase std::vector particles_; /// Random number generator used for sampling noise. - std::default_random_engine rng_; + std::mt19937 rng_; /// Current estimated odometry-based pose. tf2::Transform pose_; @@ -234,11 +230,13 @@ class AMCLLocalizer : public LocalizerMethodBase /// Timestamp of the last reseed event. rclcpp::Time last_reseed_; + /// Timestamp of the last input message (odometry or initial pose). + rclcpp::Time last_input_time_; + /** * @brief Internal static map. */ std::shared_ptr bonxai_map_; - ::navmap::NavMap navmap_; ::navmap::NavCelId last_cid_ {0}; // PerceptionModel percepcion_model_; diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index dda296e3..c7615292 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -20,7 +20,6 @@ /// \file /// \brief AMCLLocalizer implementation (Bonxai + probabilistic inflation + ray casting). -#include #include #include #include @@ -28,7 +27,6 @@ #include #include #include -#include #include #include @@ -40,7 +38,6 @@ #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_common/types/IMUPerception.hpp" @@ -352,6 +349,7 @@ struct CoordEq // ---------- AMCLLocalizer ---------- AMCLLocalizer::AMCLLocalizer() +: rng_(std::random_device{}()) { NavState::register_printer( [](const nav_msgs::msg::Odometry & odom) { @@ -359,7 +357,8 @@ AMCLLocalizer::AMCLLocalizer() tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); double r, p, y; tf2::Matrix3x3(q).getRPY(r, p, y); - ret << "Odometry with pose: (x: " << odom.pose.pose.position.x + ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " << + odom.pose.pose.position.x << ", y: " << odom.pose.pose.position.y << ", yaw: " << y << ")"; return ret.str(); }); @@ -367,7 +366,7 @@ AMCLLocalizer::AMCLLocalizer() AMCLLocalizer::~AMCLLocalizer() = default; -std::expected AMCLLocalizer::on_initialize() +void AMCLLocalizer::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -470,34 +469,38 @@ std::expected AMCLLocalizer::on_initialize() last_reseed_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - return {}; } void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) { if (compute_odom_from_tf_) {return;} tf2::fromMsg(msg->pose.pose, odom_); + last_input_time_ = msg->header.stamp; if (!initialized_odom_) {last_odom_ = odom_; initialized_odom_ = true;} } void AMCLLocalizer::update_odom_from_tf() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); + tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "TF failed: %s", ex.what()); return; } tf2::fromMsg(tf_msg.transform, odom_); + last_input_time_ = tf_msg.header.stamp; initialized_odom_ = true; } std::optional get_latest_imu_quat(const NavState & nav_state) { if (!nav_state.has("imu")) {return std::nullopt;} - const auto imus = nav_state.get("imu"); + const auto & imus = nav_state.get("imu"); if (imus.empty() || !imus.back()) {return std::nullopt;} const auto & imu_msg = imus.back()->data; tf2::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y, @@ -530,7 +533,9 @@ void AMCLLocalizer::predict(NavState & nav_state) tf2::Transform delta = last_odom_.inverseTimes(odom_); const bool have_navmap = nav_state.has("map.navmap"); - if (have_navmap) {navmap_ = nav_state.get<::navmap::NavMap>("map.navmap");} + + if (!have_navmap) {return;} + const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); const auto imu_q_opt = get_latest_imu_quat(nav_state); @@ -541,17 +546,16 @@ void AMCLLocalizer::predict(NavState & nav_state) double r, p, yaw; tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, yaw); double rot_len = std::abs(yaw); - std::random_device rd; std::mt19937 gen(rd()); + std::normal_distribution n_dx(0.0, std::abs(dx) * noise_translation_); + std::normal_distribution n_dy(0.0, std::abs(dy) * noise_translation_); + std::normal_distribution n_dz(0.0, std::abs(dz) * noise_translation_); + std::normal_distribution n_yaw(0.0, + rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + + double noisy_y = yaw + n_yaw(rng_); for (auto & p : particles_) { - std::normal_distribution n_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution n_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution n_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution n_yaw(0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); - - tf2::Vector3 noisy_t(dx + n_dx(gen), dy + n_dy(gen), dz + n_dz(gen)); - double noisy_y = yaw + n_yaw(gen); + tf2::Vector3 noisy_t(dx + n_dx(rng_), dy + n_dy(rng_), dz + n_dz(rng_)); tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_y); p.pose = p.pose * tf2::Transform(noisy_q, noisy_t); @@ -566,9 +570,9 @@ void AMCLLocalizer::predict(NavState & nav_state) std::size_t sidx = 0; ::navmap::NavCelId cid = std::numeric_limits::max(); Eigen::Vector3f bary, hit_eig; - const bool ok = navmap_.locate_navcel( + const bool ok = navmap.locate_navcel( Eigen::Vector3f(static_cast(Pw.x()), static_cast(Pw.y()), - static_cast(Pw.z() + 0.5f)), + static_cast(Pw.z())), sidx, cid, bary, &hit_eig, opts); if (ok) { @@ -577,7 +581,7 @@ void AMCLLocalizer::predict(NavState & nav_state) if (imu_q_opt.has_value()) { p.pose.setRotation(*imu_q_opt); } else { - const auto & cel = navmap_.navcels[cid]; + const auto & cel = navmap.navcels[cid]; tf2::Vector3 n_world = to_tf(cel.normal); double nlen = n_world.length(); if (nlen > 1e-9) { @@ -618,12 +622,14 @@ static inline std::string get_frame_id_from(const PointPerception & pp) } -static inline tf2::Transform lookup_bf_to_sensor(const std::string & sensor_frame) +static inline tf2::Transform lookup_bf_to_sensor( + const std::string & robot_frame, + const std::string & sensor_frame) { if (sensor_frame.empty()) {return tf2::Transform::getIdentity();} geometry_msgs::msg::TransformStamped tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "base_footprint", sensor_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); + robot_frame, sensor_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); tf2::Transform T; tf2::fromMsg(tf_msg.transform, T); return T; } @@ -687,12 +693,11 @@ static ScoreAgg score_particle_sensor_cloud( // ---------- correct() ---------- void AMCLLocalizer::correct(NavState & nav_state) { - auto t0 = get_node()->now(); if (!nav_state.has("points")) { RCLCPP_WARN(get_node()->get_logger(), "No points perceptions yet"); return; } - const auto perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get("points"); if (!nav_state.has("map.bonxai")) { RCLCPP_WARN(get_node()->get_logger(), "No Bonxai map yet"); @@ -701,7 +706,7 @@ void AMCLLocalizer::correct(NavState & nav_state) // Build inflated map once and cache in NavState if (!nav_state.has("map.bonxai.inflated")) { - const auto original_map = nav_state.get_ptr("map.bonxai"); + const auto & original_map = nav_state.get_ptr("map.bonxai"); auto logger = get_node()->get_logger(); auto clock = get_node()->get_clock(); auto progress_cb = [logger, clock](float f) { @@ -717,14 +722,12 @@ void AMCLLocalizer::correct(NavState & nav_state) nav_state.set("map.bonxai.inflated", inflated_map); } - const auto original_map = nav_state.get_ptr("map.bonxai"); - const auto inflated_map = nav_state.get_ptr("map.bonxai.inflated"); + const auto & original_map = nav_state.get_ptr("map.bonxai"); + const auto & inflated_map = nav_state.get_ptr("map.bonxai.inflated"); // Compute tail-skip coherent with inflation kernel const int TAIL_SKIP = compute_tail_skip(*original_map, inflation_stddev_, inflation_prob_min_); - auto t1 = get_node()->now(); - // Prepare per-sensor bundles (downsample/cap per sensor) std::vector bundles; bundles.reserve(perceptions.size()); for (const auto & h : perceptions) { @@ -743,8 +746,10 @@ void AMCLLocalizer::correct(NavState & nav_state) T_bf_sensor_cache.reserve(bundles.size()); for (const auto & b : bundles) { if (!T_bf_sensor_cache.count(b.frame_id)) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); try { - T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(b.frame_id); + T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(tf_info.robot_frame, + b.frame_id); // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; @@ -756,8 +761,6 @@ void AMCLLocalizer::correct(NavState & nav_state) } } - auto t2 = get_node()->now(); - // Score all particles against all sensors bool debug = false; for (auto & particle : particles_) { @@ -776,8 +779,6 @@ void AMCLLocalizer::correct(NavState & nav_state) particle.possible_hits += possible; } - auto t3 = get_node()->now(); - // Weights update with power tau for (auto & particle : particles_) { if (particle.possible_hits > 0) { @@ -798,18 +799,6 @@ void AMCLLocalizer::correct(NavState & nav_state) p.weight /= total_w; } } - - auto t4 = get_node()->now(); - - // Light profiling - std::cerr << "Prepare maps: " << std::fixed << std::setprecision(6) << (t1 - t0).seconds() << - " s\n"; - std::cerr << "Prep sensors: " << std::fixed << std::setprecision(6) << (t2 - t1).seconds() << - " s\n"; - std::cerr << "Scoring: " << std::fixed << std::setprecision(6) << (t3 - t2).seconds() << - " s\n"; - std::cerr << "Weights: " << std::fixed << std::setprecision(6) << (t4 - t3).seconds() << - " s\n"; } // ------------------- reseed ------------------- @@ -830,13 +819,6 @@ AMCLLocalizer::reseed() std::sort(particles_.begin(), particles_.end(), [](const Particle & a, const Particle & b) {return a.weight > b.weight;}); - // std::cerr << - // "=================================================================================\n"; - // for (std::size_t i = 0; i < particles_.size(); i++) { - // std::cerr << "[" << i << "] " << particles_[i].hits << "/" << particles_[i].possible_hits - // << " " << static_cast(particles_[i].last_cid) << std::endl; - // } - tf2::Vector3 mean = computeMean(particles_, 0, N_top); tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); @@ -976,9 +958,10 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) return; } geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; + tf_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + tf_msg.header.frame_id = tf_info.map_frame; + tf_msg.child_frame_id = tf_info.odom_frame; tf_msg.transform = tf2::toMsg(map2bf); RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); tf_broadcaster_->sendTransform(tf_msg); @@ -986,9 +969,11 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) void AMCLLocalizer::publishParticles() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; + array_msg.header.stamp = last_input_time_; + array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { geometry_msgs::msg::Pose pose_msg; @@ -1001,34 +986,91 @@ void AMCLLocalizer::publishParticles() particles_pub_->publish(array_msg); } -tf2::Transform AMCLLocalizer::getEstimatedPose() const +tf2::Transform +AMCLLocalizer::getEstimatedPose() const { if (particles_.empty()) {return tf2::Transform::getIdentity();} - const std::size_t N = particles_.size(), N_top = N / 2; - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b){return a.weight > b.weight;}); - tf2::Vector3 mean = computeMean(sorted, 0, N_top); + + const std::size_t N = particles_.size(); + std::size_t N_top = N / 2; + if (N_top == 0) { + N_top = 1; + } + + std::vector idx(N); + std::iota(idx.begin(), idx.end(), 0); + + std::nth_element( + idx.begin(), + idx.begin() + N_top, + idx.end(), + [this](std::size_t a, std::size_t b) { + return particles_[a].weight > particles_[b].weight; + }); + + std::vector top_particles; + top_particles.reserve(N_top); + for (std::size_t k = 0; k < N_top; ++k) { + top_particles.push_back(particles_[idx[k]]); + } + + tf2::Vector3 mean = computeMean(top_particles, 0, top_particles.size()); if (!std::isfinite(mean.x()) || !std::isfinite(mean.y()) || !std::isfinite(mean.z())) { mean = tf2::Vector3(0, 0, 0); } - double r, p, y; tf2::Matrix3x3(sorted[0].pose.getRotation()).getRPY(r, p, y); - tf2::Quaternion q; q.setRPY(0.0, 0.0, y); - tf2::Transform est; est.setOrigin(mean); est.setRotation(q); return est; + + const Particle & best = top_particles[0]; + double r, p, y; + tf2::Matrix3x3(best.pose.getRotation()).getRPY(r, p, y); + tf2::Quaternion q; + q.setRPY(0.0, 0.0, y); + + tf2::Transform est; + est.setOrigin(mean); + est.setRotation(q); + return est; } -void AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) + +void +AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) { if (particles_.empty()) {return;} - const std::size_t N = particles_.size(), N_top = N / 2; + + const std::size_t N = particles_.size(); + std::size_t N_top = N / 2; + if (N_top == 0) { + N_top = 1; + } + + std::vector idx(N); + std::iota(idx.begin(), idx.end(), 0); + + std::nth_element( + idx.begin(), + idx.begin() + N_top, + idx.end(), + [this](std::size_t a, std::size_t b) { + return particles_[a].weight > particles_[b].weight; + }); + + std::vector top_particles; + top_particles.reserve(N_top); + for (std::size_t k = 0; k < N_top; ++k) { + top_particles.push_back(particles_[idx[k]]); + } tf2::Vector3 mean = est_pose.getOrigin(); - tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); - double yaw_var = computeYawVariance(particles_, 0, N_top); + + tf2::Matrix3x3 cov = computeCovariance(top_particles, 0, top_particles.size(), mean); + double yaw_var = computeYawVariance(top_particles, 0, top_particles.size()); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.stamp = last_input_time_; + msg.header.frame_id = tf_info.map_frame; + msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); msg.pose.pose.position.z = mean.z(); @@ -1039,32 +1081,57 @@ void AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) msg.pose.covariance[6 * r + c] = cov[r][c]; } } + msg.pose.covariance[6 * 5 + 5] = yaw_var; + estimate_pub_->publish(msg); } -nav_msgs::msg::Odometry AMCLLocalizer::get_pose() +nav_msgs::msg::Odometry +AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; + odom_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_msg.header.frame_id = tf_info.map_frame; + odom_msg.child_frame_id = tf_info.robot_frame; pose_ = getEstimatedPose(); tf2::Transform est_pose = pose_; + odom_msg.pose.pose.position.x = est_pose.getOrigin().x(); odom_msg.pose.pose.position.y = est_pose.getOrigin().y(); odom_msg.pose.pose.position.z = est_pose.getOrigin().z(); odom_msg.pose.pose.orientation = tf2::toMsg(est_pose.getRotation()); if (!particles_.empty()) { - const std::size_t N = particles_.size(), N_top = N / 2; - std::vector sorted = particles_; - std::sort(sorted.begin(), sorted.end(), - [](const Particle & a, const Particle & b){return a.weight > b.weight;}); - tf2::Vector3 mean = computeMean(sorted, 0, N_top); - tf2::Matrix3x3 cov = computeCovariance(sorted, 0, N_top, mean); - double yaw_var = computeYawVariance(sorted, 0, N_top); + const std::size_t N = particles_.size(); + std::size_t N_top = N / 2; + if (N_top == 0) { + N_top = 1; + } + + std::vector idx(N); + std::iota(idx.begin(), idx.end(), 0); + + std::nth_element( + idx.begin(), + idx.begin() + N_top, + idx.end(), + [this](std::size_t a, std::size_t b) { + return particles_[a].weight > particles_[b].weight; + }); + + std::vector top_particles; + top_particles.reserve(N_top); + for (std::size_t k = 0; k < N_top; ++k) { + top_particles.push_back(particles_[idx[k]]); + } + + tf2::Vector3 mean = computeMean(top_particles, 0, top_particles.size()); + tf2::Matrix3x3 cov = computeCovariance(top_particles, 0, top_particles.size(), mean); + double yaw_var = computeYawVariance(top_particles, 0, top_particles.size()); + for (int r = 0; r < 3; ++r) { for (int c = 0; c < 3; ++c) { odom_msg.pose.covariance[6 * r + c] = cov[r][c]; @@ -1079,6 +1146,7 @@ nav_msgs::msg::Odometry AMCLLocalizer::get_pose() odom_msg.twist.twist.angular.x = 0.0; odom_msg.twist.twist.angular.y = 0.0; odom_msg.twist.twist.angular.z = 0.0; + return odom_msg; } diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index 5423c281..5b61ef63 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_simple_common REQUIRED) diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 04505e89..481d6830 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -1,21 +1,25 @@ # easynav_simple_localizer -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + AMCL-style localizer using a simple 2D grid-like map for scoring and odometry for prediction. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_simple_localizer/AMCLLocalizer` - **Type:** `easynav::AMCLLocalizer` - **Base Class:** `easynav::LocalizerMethodBase` @@ -23,6 +27,7 @@ AMCL-style localizer using a simple 2D grid-like map for scoring and odometry fo - **Description:** AMCL-style localizer using a simple 2D grid-like map for scoring and odometry for prediction. ## Parameters + All parameters are declared under the plugin namespace, i.e., `//easynav_simple_localizer/AMCLLocalizer/...`. | Name | Type | Default | Description | @@ -40,33 +45,34 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.min_noise_xy` | `double` | `0.05` | Minimum XY noise (m). | | `.min_noise_yaw` | `double` | `0.05` | Minimum yaw noise (rad). | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `/odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false (not present here). | SensorDataQoS (reliable) | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set. | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Estimated pose with covariance. | depth=10 | - ### Services + This package does not create service servers or clients. ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | | `map.static` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Estimated robot pose. | - ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `map -> odom` | Aligns the odometry frame with the map frame. | - ## License + GPL-3.0-only diff --git a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp index 22f60ab9..7bff4232 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -24,11 +24,6 @@ #define EASYNAV_SIMPLE_LOCALIZER__AMCLLOCALIZER_HPP_ #include -#include -#include -#include -#include -#include #include #include @@ -72,9 +67,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -209,6 +204,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Timestamp of the last reseed event. rclcpp::Time last_reseed_; + + /// Timestamp of the last input message (odometry or initial pose). + rclcpp::Time last_input_time_; }; } // namespace easynav diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index cc5c05f6..693d006e 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -20,14 +20,12 @@ /// \file /// \brief Implementation of the AMCLLocalizer class. -#include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Vector3.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_simple_common/SimpleMap.hpp" @@ -177,7 +175,8 @@ AMCLLocalizer::AMCLLocalizer() double roll, pitch, yaw; tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - ret << "Odometry with pose: (x: " << x << ", y: " << y << ", yaw: " << yaw << ")"; + ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << "} Odometry with pose: (x: " << + x << ", y: " << y << ", yaw: " << yaw << ")"; return ret.str(); }); } @@ -186,7 +185,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +void AMCLLocalizer::on_initialize() { auto node = get_node(); @@ -264,8 +263,6 @@ AMCLLocalizer::on_initialize() last_reseed_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - - return {}; } void printTransform(const tf2::Transform & tf) @@ -312,6 +309,7 @@ void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) { tf2::fromMsg(msg->pose.pose, odom_); + last_input_time_ = msg->header.stamp; if (!initialized_odom_) { last_odom_ = odom_; @@ -378,7 +376,7 @@ void AMCLLocalizer::correct(NavState & nav_state) return; } - const auto perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get("points"); if (!nav_state.has("map.static")) { RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); @@ -387,9 +385,10 @@ void AMCLLocalizer::correct(NavState & nav_state) const auto & map_static = nav_state.get("map.static"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.resolution()) - .fuse(get_tf_prefix() + "base_footprint") + .fuse(tf_info.robot_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.resolution()) @@ -526,9 +525,10 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); - tf_msg.header.frame_id = get_tf_prefix() + "map"; - tf_msg.child_frame_id = get_tf_prefix() + "odom"; + tf_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + tf_msg.header.frame_id = tf_info.map_frame; + tf_msg.child_frame_id = tf_info.odom_frame; tf_msg.transform = tf2::toMsg(map2bf); RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false); @@ -538,9 +538,11 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf) void AMCLLocalizer::publishParticles() { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseArray array_msg; - array_msg.header.stamp = get_node()->now(); - array_msg.header.frame_id = get_tf_prefix() + "map"; + array_msg.header.stamp = last_input_time_; + array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { @@ -599,9 +601,11 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) tf2::Matrix3x3 cov = computeCovariance(particles_, 0, N_top, mean); double yaw_variance = computeYawVariance(particles_, 0, N_top); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.stamp = last_input_time_; + msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -623,9 +627,10 @@ AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); - odom_msg.header.frame_id = get_tf_prefix() + "map"; - odom_msg.child_frame_id = get_tf_prefix() + "base_footprint"; + odom_msg.header.stamp = last_input_time_; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + odom_msg.header.frame_id = tf_info.map_frame; + odom_msg.child_frame_id = tf_info.robot_frame; tf2::Transform est_pose = getEstimatedPose(); diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index abf70ba0..731a256e 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -104,6 +104,13 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) { auto node = std::make_shared("test_node2"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test2"); rclcpp::executors::SingleThreadedExecutor executor; @@ -114,7 +121,7 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; + grid.header.frame_id = tf_info.map_frame; grid.info.width = 10; grid.info.height = 10; grid.info.resolution = 0.2; diff --git a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt index 994a44ea..eea955ee 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) diff --git a/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index 4df2fba2..6f08a413 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -1,21 +1,25 @@ # easynav_bonxai_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) probabilistic 3D occupancy map and exposes it via ROS topics, NavState, and a save-map service. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_bonxai_maps_manager/BonxaiMapsManager` - **Type:** `easynav_bonxai::BonxaiMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -25,6 +29,7 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) ## Parameters ### Plugin Parameters (namespace: `//easynav_bonxai_maps_manager/BonxaiMapsManager/...`) + | Name | Type | Default | Description | |---|---|---:|---| | `.package` | `string` | `""` | Package name to resolve relative map paths via ament index. | @@ -33,34 +38,34 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) | `.resolution` | `double` | `0.3` | Voxel resolution for the Bonxai map (meters). | | `.frame_id` | `string` | `"map"` | Frame ID stamped on published map messages. | - ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update the Bonxai map. | QoS depth=100 | | Subscription | `//incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid (YAML + PGM) to import into Bonxai. | QoS depth=1, transient_local, reliable | | Publisher | `//map` | `sensor_msgs/msg/PointCloud2` | Published Bonxai map as a point cloud. | QoS depth=1, transient_local, reliable | - ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Save current Bonxai map to disk. | - ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `map.bonxai` | `Bonxai::ProbabilisticMap` | **Write** | Stores the current Bonxai map instance. | - ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | `—` | No TF broadcast; outputs use the configured frame_id. | - ## License + GPL-3.0-only diff --git a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp index 5d0d84c4..8a81e0f1 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp @@ -23,29 +23,14 @@ #ifndef EASYNAV_BONXAI_MAPS_MANAGER__BONXAI_MAPS_MANAGER_HPP_ #define EASYNAV_BONXAI_MAPS_MANAGER__BONXAI_MAPS_MANAGER_HPP_ -#include -#include -#include -#include -#include -#include - -#include "bonxai/bonxai.hpp" #include "bonxai/probabilistic_map.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" -#include "pluginlib/class_loader.hpp" - -#include "yaets/tracing.hpp" - namespace easynav_bonxai { @@ -76,9 +61,9 @@ class BonxaiMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -133,7 +118,6 @@ class BonxaiMapsManager : public easynav::MapsManagerBase double resolution_ {0.3}; double height_with_occ_ {1.0}; - std::string frame_id_ {"map"}; }; } // namespace easynav_bonxai diff --git a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/utils.hpp b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/utils.hpp deleted file mode 100644 index 8e582124..00000000 --- a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/utils.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#ifndef EASYNAV_BONXAI_MAPS_MANAGER_UTILS_HPP_ -#define EASYNAV_BONXAI_MAPS_MANAGER_UTILS_HPP_ - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav_bonxai -{ - - -} // namespace easynav_bonxai -#endif // EASYNAV_BONXAI_MAPS_MANAGER_UTILS_HPP_ diff --git a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp index 4f8182c6..bc8c3e4f 100644 --- a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp +++ b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp @@ -17,14 +17,10 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include #include #include "easynav_bonxai_maps_manager/BonxaiMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" -#include "easynav_common/YTSession.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "bonxai/bonxai.hpp" @@ -34,7 +30,6 @@ #include "pcl_conversions/pcl_conversions.h" #include "pcl/point_types.h" #include "pcl/point_cloud.h" -#include "pcl/filters/voxel_grid.h" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -57,7 +52,7 @@ BonxaiMapsManager::BonxaiMapsManager() BonxaiMapsManager::~BonxaiMapsManager() {} -std::expected +void BonxaiMapsManager::on_initialize() { auto node = get_node(); @@ -68,13 +63,11 @@ BonxaiMapsManager::on_initialize() node->declare_parameter(plugin_name + ".bonxai_path_file", bonxai_path_file); node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->declare_parameter(plugin_name + ".resolution", resolution_); - node->declare_parameter(plugin_name + ".frame_id", frame_id_); node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".bonxai_path_file", bonxai_path_file); node->get_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->get_parameter(plugin_name + ".resolution", resolution_); - node->get_parameter(plugin_name + ".frame_id", frame_id_); bonxai_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", @@ -87,7 +80,7 @@ BonxaiMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + bonxai_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } std::vector bonxai_result; @@ -120,13 +113,13 @@ BonxaiMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } nav_msgs::msg::OccupancyGrid occ_msg; if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } update_from_occ(occ_msg); @@ -165,8 +158,6 @@ BonxaiMapsManager::on_initialize() Bonxai::WritePointsFromPCD(map_path_, bonxai_result); // This can overwrite yaml occ maps // ToDo }); - - return {}; } void @@ -181,11 +172,13 @@ BonxaiMapsManager::update(::easynav::NavState & nav_state) void BonxaiMapsManager::update_from_pc2(const sensor_msgs::msg::PointCloud2 & pc2) { + const auto & tf_info = ::easynav::RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = ::easynav::RTTFBuffer::getInstance()->lookupTransform( - frame_id_, pc2.header.frame_id, pc2.header.stamp, rclcpp::Duration::from_seconds(0.05)); + tf_info.map_frame, pc2.header.frame_id, pc2.header.stamp, + rclcpp::Duration::from_seconds(0.05)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "OctomapMapsManager: TF failed: %s", ex.what()); return; @@ -289,7 +282,9 @@ BonxaiMapsManager::update_from_occ(const nav_msgs::msg::OccupancyGrid & occ) void BonxaiMapsManager::publish_map() { - bonxai_msg_.header.frame_id = frame_id_; + const auto & tf_info = ::easynav::RTTFBuffer::getInstance()->get_tf_info(); + + bonxai_msg_.header.frame_id = tf_info.map_frame; bonxai_msg_.header.stamp = this->get_node()->now(); bonxai_pub_->publish(bonxai_msg_); diff --git a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/utils.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/utils.cpp deleted file mode 100644 index 59339434..00000000 --- a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/utils.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "easynav_bonxai_maps_manager/utils.hpp" - -namespace easynav_bonxai -{ - - -} // namespace easynav_bonxai diff --git a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt index 6ef5ac5b..15c6c401 100644 --- a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index e3a54d13..d2df322a 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -1,11 +1,11 @@ # easynav_costmap_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Maps Manager that maintains 2D costmaps (static and dynamic), supports filter plugins (such as inflation and obstacle filters), and exposes maps through ROS topics and NavState integration. At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends the binary occupancy grid into a graded cost representation with values in the range [0–255]: + - 0: Free space, no cost to traverse. - 1–252: Gradual cost values, representing increasing difficulty or proximity to obstacles. - 253: “Near obstacle” (inscribed obstacle) cost, traversal strongly discouraged. @@ -13,16 +13,21 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends - 255: Unknown space. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_costmap_maps_manager/CostmapMapsManager` - **Type:** `easynav::CostmapMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -34,6 +39,7 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends ## Parameters ### Plugin Parameters (namespace: `//easynav_costmap_maps_manager/CostmapMapsManager/...`) + | Name | Type | Default | Description | |---|---|---:|---| | `.package` | `string` | `""` | Package name used to resolve relative map paths via `ament_index`. | @@ -43,19 +49,60 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends --- -### Filter Parameters -Each entry in `.filters` defines a sub-namespace `.` with at least the key `plugin`, plus any specific parameters. +### Filter Plugins -#### InflationFilter -| Name | Type | Default | Description | +Each entry in `.filters` defines a sub-namespace `.` with at least the key `plugin`, plus any filter-specific parameters. + +#### **ObstacleFilter** + +- **Plugin Name:** `easynav_costmap_maps_manager/ObstacleFilter` +- **Type:** `easynav::ObstacleFilter` +- **Description:** + Detects occupied cells from input point clouds (`points` key in `NavState`) and marks them as `LETHAL_OBSTACLE` in the dynamic costmap. + The filter fuses incoming 3D points into the map frame, downsamples them to the costmap resolution, filters out ground-level points (z < 0.1 m), and sets corresponding cells to lethal cost. Additionally, it computes and stores bounding box (`ObstacleBounds`) of updated obstacles to enable efficient incremental inflation. + +**Parameters:** + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| _(None)_ | — | — | This filter does not declare additional ROS parameters beyond `plugin`. Downsampling resolution and frame fusion use the costmap's own resolution and `map` frame. | + +**NavState Keys:** + +| Key | Type | Access | Description | +|---|---|---|---| +| `points` | `sensor_msgs::msg::PointCloud2` | **Read** | Input point clouds to detect obstacles. | +| `map.dynamic.filtered` | `Costmap2D` | **Write** | Marks cells as `LETHAL_OBSTACLE` (254). | +| `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Write** | Bounding box of updated obstacles for incremental inflation. | + +#### **InflationFilter** + +- **Plugin Name:** `easynav_costmap_maps_manager/InflationFilter` +- **Type:** `easynav::InflationFilter` +- **Description:** + Expands obstacle information in the costmap by assigning graded costs around `LETHAL_OBSTACLE` cells based on distance. Uses a breadth-first wavefront propagation algorithm (distance bins) to efficiently inflate obstacles up to `inflation_radius`. + The filter reads both the static map and the dynamic filtered map, applies inflation to each, and merges results. If `ObstacleBounds` is available in `NavState`, inflation is restricted to the updated region for performance. + +**Parameters:** + +| Parameter | Type | Default | Description | |---|---|---:|---| -| `.inflation.inflation_radius` | `double` | `0.3` | Radius (m) used to inflate obstacles in the costmap. | -| `.inflation.cost_scaling_factor` | `double` | `3.0` | Exponential decay factor controlling cost reduction with distance. | +| `.inflation_radius` | `double` | `0.3` | Maximum inflation distance (m) from obstacles. Cells farther than this receive no inflation cost. | +| `.inscribed_radius` | `double` | `0.25` | Radius of the inscribed zone (m). Cells within this distance of an obstacle are marked with high constant cost (`INSCRIBED_INFLATED_OBSTACLE`, value 253) before exponential decay begins. | +| `.cost_scaling_factor` | `double` | `3.0` | Exponential decay rate controlling how quickly cost decreases with distance beyond the inscribed radius. Higher values produce steeper cost gradients. | + +**NavState Keys:** -#### ObstacleFilter -This filter does not declare any ROS parameters apart from `plugin`. +| Key | Type | Access | Description | +|---|---|---|---| +| `map.static` | `Costmap2D` | **Read** | Static costmap to inflate. | +| `map.dynamic.filtered` | `Costmap2D` | **Read/Write** | Dynamic costmap input and output after inflation. | +| `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Read** (optional) | Restricts inflation to updated region for performance. | + +**Cost Model:** +Uses exponential decay: `cost = exp(-cost_scaling_factor * (distance - inscribed_radius)) * 253` for distances beyond inscribed radius. -**Example Configuration** +### Example Configuration ```yaml maps_manager_node: @@ -79,6 +126,7 @@ maps_manager_node: ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `//incoming_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy map used to update the dynamic map. | `depth=1, transient_local, reliable` | @@ -86,6 +134,7 @@ maps_manager_node: | Publisher | `//dynamic_map` | `nav_msgs/msg/OccupancyGrid` | Publishes the dynamic (live) costmap. | `depth=100` | ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves the current costmap(s) to disk. | @@ -93,15 +142,18 @@ maps_manager_node: --- ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `map.static` | `Costmap2D` | **Write** | Static map loaded from YAML. | | `map.dynamic` | `Costmap2D` | **Write** | Dynamic map after applying filters. | | `map.dynamic.filtered` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | — | This manager does not broadcast TF; costmaps use their internal `frame_id`. | @@ -109,4 +161,5 @@ maps_manager_node: --- ## License + GPL-3.0-only diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp index 3499a0e6..f4d0fb58 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp @@ -24,26 +24,16 @@ #define EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ #include -#include -#include -#include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" #include "pluginlib/class_loader.hpp" -#include "yaets/tracing.hpp" - namespace easynav { @@ -73,9 +63,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -106,6 +96,11 @@ class CostmapMapsManager : public easynav::MapsManagerBase */ Costmap2D static_map_; + /** + * @brief Internal static map. + */ + std::shared_ptr dynamic_map_; + /** * @brief Publisher for the static occupancy grid. */ diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp index 645fde85..0ede2b6a 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp @@ -21,10 +21,8 @@ #ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ -#include #include -#include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -32,31 +30,33 @@ namespace easynav { +struct ObstacleBounds +{ + double min_x, min_y, max_x, max_y; +}; + + class CostmapFilter { public: CostmapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix = ""); + const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(NavState & nav_state) = 0; -protected: - std::shared_ptr get_node() const; - const std::string & get_plugin_name() const; - const std::string & get_tf_prefix() const; +protected: + std::shared_ptr get_node() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; }; } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp index 56275fbc..20e9a086 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp @@ -42,14 +42,8 @@ #ifndef EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ -#include #include -#include #include -#include -#include - -#include "pluginlib/class_loader.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -84,7 +78,7 @@ class InflationFilter : public CostmapFilter public: InflationFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(NavState & nav_state); /** @@ -202,7 +196,13 @@ class InflationFilter : public CostmapFilter } /** - * @brief Enqueue new cells in cache distance update search + * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation + * @param grid The costmap + * @param index The index of the cell + * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) + * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) + * @param src_x The x index of the obstacle point inflation started at + * @param src_y The y index of the obstacle point inflation started at */ inline void enqueue( unsigned int index, unsigned int mx, unsigned int my, @@ -229,6 +229,21 @@ class InflationFilter : public CostmapFilter bool need_reinflation_; bool matchedSize_ {false}; + + Costmap2D static_inflated_; + bool has_static_inflated_ {false}; + + struct StaticGeomSignature + { + unsigned int size_x {0}; + unsigned int size_y {0}; + double resolution {0.0}; + double origin_x {0.0}; + double origin_y {0.0}; + } static_sig_; + + bool needs_recompute_static_(const Costmap2D & static_map) const; + void recompute_static_inflation_(const Costmap2D & static_map); }; } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp index a73d5848..940a2e18 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp @@ -21,12 +21,8 @@ #ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ -#include #include -#include "pluginlib/class_loader.hpp" - -#include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" @@ -39,7 +35,7 @@ class ObstacleFilter : public CostmapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(NavState & nav_state); }; diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index de66b203..229846b0 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -17,18 +17,16 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include "easynav_costmap_maps_manager/CostmapMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" #include "easynav_costmap_common/costmap_2d.hpp" -#include "easynav_costmap_common/cost_values.hpp" #include "easynav_costmap_maps_manager/map_io.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_prefix.hpp" @@ -54,7 +52,7 @@ CostmapMapsManager::CostmapMapsManager() CostmapMapsManager::~CostmapMapsManager() {} -std::expected +void CostmapMapsManager::on_initialize() { auto node = get_node(); @@ -83,14 +81,13 @@ CostmapMapsManager::on_initialize() std::shared_ptr instance; instance = costmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + costmap_filter, - get_tf_prefix()); - - if (!result) { + try { + instance->initialize(node, plugin_name + "." + costmap_filter); + } catch (std::runtime_error & ex) { RCLCPP_ERROR(node->get_logger(), - "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - return std::unexpected("Unable to initialize " + - plugin + " . Error: " + result.error()); + "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); + throw std::runtime_error("Unable to initialize " + + plugin + " . Error: " + ex.what()); } costmap_filters_.push_back(instance); @@ -100,7 +97,7 @@ CostmapMapsManager::on_initialize() } catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR(node->get_logger(), "Unable to load plugin easynav::CostmapFilter. Error: %s", ex.what()); - return std::unexpected("Unable to load plugin easynav::CostmapFilter " + + throw std::runtime_error("Unable to load plugin easynav::CostmapFilter " + costmap_filter + " . Error: " + ex.what()); } } @@ -112,23 +109,25 @@ CostmapMapsManager::on_initialize() dynamic_occ_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + map_path_ = "/tmp/default.map.yaml"; if (!package_name.empty() && !map_path_file.empty()) { try { const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + map_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (auto ret = loadMapFromYaml(map_path_, static_grid_msg_) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } static_map_ = Costmap2D(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); } @@ -136,12 +135,12 @@ CostmapMapsManager::on_initialize() incoming_map_sub_ = node->create_subscription( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), - [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { + [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { static_grid_msg_ = *msg; static_map_ = Costmap2D(*msg); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = this->get_node()->now(); static_occ_pub_->publish(static_grid_msg_); @@ -149,7 +148,7 @@ CostmapMapsManager::on_initialize() savemap_srv_ = node->create_service( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", - [this]( + [&]( const std::shared_ptr request, std::shared_ptr response) { @@ -170,8 +169,6 @@ CostmapMapsManager::on_initialize() response->message = "Map successfully saved to: " + map_path_; } }); - - return {}; } void @@ -190,19 +187,31 @@ CostmapMapsManager::update(NavState & nav_state) nav_state.set("map.static", static_map_); } - Costmap2D dynamic_map = static_map_; - nav_state.set("map.dynamic.filtered", dynamic_map); + if (!dynamic_map_) { + dynamic_map_ = std::make_shared(static_map_); + } else { + *dynamic_map_ = static_map_; + } + + nav_state.set("map.dynamic.filtered", dynamic_map_); + + if (!nav_state.has("map_time")) { + nav_state.set("map_time", get_node()->now()); + } for (const auto & filter : costmap_filters_) { filter->update(nav_state); } - const auto & final_dynamic_map = nav_state.get("map.dynamic.filtered"); - nav_state.set("map.dynamic", final_dynamic_map); + nav_state.set("map.dynamic", dynamic_map_); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + rclcpp::Time map_stamp = nav_state.get("map_time"); - final_dynamic_map.toOccupancyGridMsg(dynamic_grid_msg_); - dynamic_grid_msg_.header.frame_id = get_tf_prefix() + "map"; - dynamic_grid_msg_.header.stamp = get_node()->now(); + dynamic_map_->toOccupancyGridMsg(dynamic_grid_msg_); + dynamic_grid_msg_.header.frame_id = tf_info.map_frame; + dynamic_grid_msg_.header.stamp = map_stamp; dynamic_occ_pub_->publish(dynamic_grid_msg_); } diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp index cfb320ec..b172a165 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp @@ -18,12 +18,8 @@ // along with this program. If not, see . -#include #include -#include "easynav_costmap_common/costmap_2d.hpp" -#include "easynav_common/types/NavState.hpp" - #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" namespace easynav @@ -34,18 +30,15 @@ CostmapFilter::CostmapFilter() } -std::expected +void CostmapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix -) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; - return on_initialize(); + on_initialize(); } std::shared_ptr @@ -60,10 +53,5 @@ CostmapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -CostmapFilter::get_tf_prefix() const -{ - return tf_prefix_; -} } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp index 372049e0..4aeffd9a 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp @@ -39,13 +39,10 @@ *********************************************************************/ -#include #include #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" @@ -73,35 +70,43 @@ InflationFilter::InflationFilter() { } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); inflation_radius_ = 0.3; + inscribed_radius_ = 0.25; cost_scaling_factor_ = 3.0; node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); + node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); + node->get_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); RCLCPP_INFO(node->get_logger(), - "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", - inflation_radius_, cost_scaling_factor_); + "InflationFilter with inflation_radius = %lf inscribed_radius = %lf cost_scaling_factor = %lf", + inflation_radius_, inscribed_radius_, cost_scaling_factor_); seen_.clear(); cached_distances_.clear(); cached_costs_.clear(); need_reinflation_ = false; - - return {}; } void InflationFilter::update(NavState & nav_state) { - Costmap2D dynamic_map = nav_state.get("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + Costmap2D & dynamic_map = *dynamic_map_ptr; + + const auto & static_map = nav_state.get("map.static"); + + if (needs_recompute_static_(static_map)) { + recompute_static_inflation_(static_map); + } if (!matchedSize_) { cell_inflation_radius_ = cellDistance(dynamic_map, inflation_radius_); @@ -109,11 +114,48 @@ InflationFilter::update(NavState & nav_state) matchedSize_ = true; } - updateCosts(dynamic_map, 0, 0, dynamic_map.getSizeInCellsX(), dynamic_map.getSizeInCellsY()); + int min_i = 0; + int min_j = 0; + int size_x = dynamic_map.getSizeInCellsX(); + int size_y = dynamic_map.getSizeInCellsY(); + int max_i = size_x; + int max_j = size_y; - nav_state.set("map.dynamic.filtered", dynamic_map); -} + if (nav_state.has("map.dynamic.obstacle_bounds")) { + const auto & bb = nav_state.get("map.dynamic.obstacle_bounds"); + unsigned int cmin_i, cmin_j, cmax_i, cmax_j; + if (dynamic_map.worldToMap(bb.min_x, bb.min_y, cmin_i, cmin_j) && + dynamic_map.worldToMap(bb.max_x, bb.max_y, cmax_i, cmax_j)) + { + min_i = static_cast(cmin_i); + min_j = static_cast(cmin_j); + max_i = static_cast(cmax_i) + 1; + max_j = static_cast(cmax_j) + 1; + + // Expand by the inflation radius in cells, so that all cells whose + // cost may be affected by the new obstacles are included. + const int r = static_cast(cell_inflation_radius_); + min_i = std::max(0, min_i - r); + min_j = std::max(0, min_j - r); + max_i = std::min(max_i + r, size_x); + max_j = std::min(max_j + r, size_y); + } + } + + updateCosts(dynamic_map, min_i, min_j, max_i, max_j); + + for (int i = 0; i < dynamic_map.getSizeInCellsX(); i++) { + for (int j = 0; j < dynamic_map.getSizeInCellsY(); j++) { + int index = static_cast(dynamic_map.getIndex(i, j)); + unsigned char cost = std::max( + dynamic_map.getCost(i, j), static_inflated_.getCost(i, j)); + dynamic_map.setCost(i, j, cost); + } + } + + nav_state.set("map.dynamic.filtered", dynamic_map_ptr); +} void InflationFilter::matchSize(easynav::Costmap2D & costmap) @@ -156,107 +198,125 @@ InflationFilter::updateBounds( } } - void InflationFilter::updateCosts( - easynav::Costmap2D & master_grid, int min_i, int min_j, - int max_i, - int max_j) + easynav::Costmap2D & master_grid, + int min_i, int min_j, + int max_i, int max_j) { if (cell_inflation_radius_ == 0) { return; } - // make sure the inflation list is empty at the beginning of the cycle (should always be true) - for (auto & dist : inflation_cells_) { - RCLCPP_FATAL_EXPRESSION( - parent_node_->get_logger(), - !dist.empty(), "The inflation list must be empty at the beginning of inflation"); + // Early exit: no valid region to inflate + if (min_i >= max_i || min_j >= max_j) { + return; } unsigned char * master_array = master_grid.getCharMap(); - unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); - - if (seen_.size() != size_x * size_y) { - RCLCPP_WARN( - parent_node_->get_logger(), "InflationFilter::updateCosts(): seen_ vector size is wrong"); - seen_ = std::vector(size_x * size_y, false); + const unsigned int size_x = master_grid.getSizeInCellsX(); + const unsigned int size_y = master_grid.getSizeInCellsY(); + const std::size_t cell_count = static_cast(size_x) * size_y; + // Reuse boolean seen_ vector for this call to avoid full O(N) clearing + if (seen_.size() != cell_count) { + seen_.assign(cell_count, false); + } else { + // Clear only inside the current bounded region to reduce cost + for (int j = min_j; j < max_j; ++j) { + unsigned int row = static_cast(j) * size_x; + for (int i = min_i; i < max_i; ++i) { + seen_[row + static_cast(i)] = false; + } + } } - std::fill(begin(seen_), end(seen_), false); + // Reuse all inflation distance bins instead of reallocating them + for (auto & dist_bin : inflation_cells_) { + dist_bin.clear(); + } - // We need to include in the inflation cells outside the bounding - // box min_i...max_j, by the amount cell_inflation_radius_. Cells - // up to that distance outside the box can still influence the costs - // stored in cells inside the box. + // Store the original bounding box (before expansion by the inflation radius) const int base_min_i = min_i; const int base_min_j = min_j; const int base_max_i = max_i; const int base_max_j = max_j; + + // Expand the region by the inflation radius (in cells). Cells up to that + // distance outside the original box can still influence costs inside it. min_i -= static_cast(cell_inflation_radius_); min_j -= static_cast(cell_inflation_radius_); max_i += static_cast(cell_inflation_radius_); max_j += static_cast(cell_inflation_radius_); + // Clamp expanded region to map bounds min_i = std::max(0, min_i); min_j = std::max(0, min_j); max_i = std::min(static_cast(size_x), max_i); max_j = std::min(static_cast(size_y), max_j); - // Inflation list; we append cells to visit in a list associated with - // its distance to the nearest obstacle - // We use a map to emulate the priority queue used before, - // with a notable performance boost + if (min_i >= max_i || min_j >= max_j) { + return; + } - // Start with lethal obstacles: by definition distance is 0.0 + // -------------------------------------------------------------------------- + // 1) Collect seeds (lethal obstacles and optionally unknown cells) inside + // the expanded window. These go into distance bin 0. + // -------------------------------------------------------------------------- auto & obs_bin = inflation_cells_[0]; obs_bin.reserve(200); - for (int j = min_j; j < max_j; j++) { - for (int i = min_i; i < max_i; i++) { - int index = static_cast(master_grid.getIndex(i, j)); - unsigned char cost = master_array[index]; - if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) { + + for (int j = min_j; j < max_j; ++j) { + for (int i = min_i; i < max_i; ++i) { + const unsigned int index = master_grid.getIndex(i, j); + const unsigned char cost = master_array[index]; + + if (cost == LETHAL_OBSTACLE || + (inflate_around_unknown_ && cost == NO_INFORMATION)) + { obs_bin.emplace_back(i, j, i, j); } } } - // Process cells by increasing distance; new cells are appended to the - // corresponding distance bin, so they - // can overtake previously inserted but farther away cells + // No seeds in this region → nothing to inflate + if (obs_bin.empty()) { + return; + } + + // -------------------------------------------------------------------------- + // 2) BFS over the distance bins. New cells are appended to the bin + // corresponding to their distance from the nearest obstacle. + // -------------------------------------------------------------------------- for (auto & dist_bin : inflation_cells_) { dist_bin.reserve(200); - for (std::size_t i = 0; i < dist_bin.size(); ++i) { - // Do not use iterator or for-range based loops to - // iterate though dist_bin, since it's size might - // change when a new cell is enqueued, invalidating all iterators - const CellData & cell = dist_bin[i]; - unsigned int mx = cell.x_; - unsigned int my = cell.y_; - unsigned int sx = cell.src_x_; - unsigned int sy = cell.src_y_; - unsigned int index = master_grid.getIndex(mx, my); - - // ignore if already visited + + for (std::size_t k = 0; k < dist_bin.size(); ++k) { + const CellData & cell = dist_bin[k]; + const unsigned int mx = cell.x_; + const unsigned int my = cell.y_; + const unsigned int sx = cell.src_x_; + const unsigned int sy = cell.src_y_; + const unsigned int index = master_grid.getIndex(mx, my); + // Process each cell only once in this call if (seen_[index]) { continue; } - seen_[index] = true; - // assign the cost associated with the distance from an obstacle to the cell - unsigned char cost = costLookup(mx, my, sx, sy); - unsigned char old_cost = master_array[index]; - // In order to avoid artifacts appeared out of boundary areas - // when some layer is going after inflation_layer, - // we need to apply inflation_layer only to inside of given bounds + // Compute inflation cost based on distance to the nearest obstacle + const unsigned char cost = costLookup(mx, my, sx, sy); + const unsigned char old_cost = master_array[index]; + + // Apply inflation only inside the original (non-expanded) bounding box if (static_cast(mx) >= base_min_i && static_cast(my) >= base_min_j && static_cast(mx) < base_max_i && static_cast(my) < base_max_j) { if (old_cost == NO_INFORMATION && - (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) + (inflate_unknown_ ? + (cost > FREE_SPACE) : + (cost >= INSCRIBED_INFLATED_OBSTACLE))) { master_array[index] = cost; } else { @@ -264,35 +324,25 @@ InflationFilter::updateCosts( } } - // attempt to put the neighbors of the current cell onto the inflation list - if (mx > 0) { - enqueue(index - 1, mx - 1, my, sx, sy); + // Enqueue 4-connected neighbors (clipped to full map bounds) + if (mx > 0u) { + enqueue(index - 1u, mx - 1u, my, sx, sy); } - if (my > 0) { - enqueue(index - size_x, mx, my - 1, sx, sy); + if (my > 0u) { + enqueue(index - size_x, mx, my - 1u, sx, sy); } - if (mx < size_x - 1) { - enqueue(index + 1, mx + 1, my, sx, sy); + if (mx + 1u < size_x) { + enqueue(index + 1u, mx + 1u, my, sx, sy); } - if (my < size_y - 1) { - enqueue(index + size_x, mx, my + 1, sx, sy); + if (my + 1u < size_y) { + enqueue(index + size_x, mx, my + 1u, sx, sy); } } - // This level of inflation_cells_ is not needed anymore. We can free the memory - // Note that dist_bin.clear() is not enough, because it won't free the memory - dist_bin = std::vector(); + + // No need to shrink dist_bin here; capacity is reused on the next call. } } -/** - * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation - * @param grid The costmap - * @param index The index of the cell - * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) - * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) - * @param src_x The x index of the obstacle point inflation started at - * @param src_y The y index of the obstacle point inflation started at - */ void InflationFilter::enqueue( unsigned int index, unsigned int mx, unsigned int my, @@ -314,6 +364,7 @@ InflationFilter::enqueue( // push the cell data onto the inflation list and mark const auto dist = distance_matrix_[mx - src_x + r][my - src_y + r]; inflation_cells_[dist].emplace_back(mx, my, src_x, src_y); + // Do not mark seen_ here; we mark upon processing to allow enqueuing neighbors } } @@ -391,6 +442,44 @@ InflationFilter::generateIntegerDistances() return level; } +bool +InflationFilter::needs_recompute_static_(const Costmap2D & static_map) const +{ + if (!has_static_inflated_) { + return true; + } + + if (static_map.getSizeInCellsX() != static_sig_.size_x || + static_map.getSizeInCellsY() != static_sig_.size_y || + static_map.getResolution() != static_sig_.resolution || + static_map.getOriginX() != static_sig_.origin_x || + static_map.getOriginY() != static_sig_.origin_y) + { + return true; + } + + return false; +} + +void +InflationFilter::recompute_static_inflation_(const Costmap2D & static_map) +{ + static_inflated_ = static_map; + + matchSize(static_inflated_); + const int w = static_inflated_.getSizeInCellsX(); + const int h = static_inflated_.getSizeInCellsY(); + updateCosts(static_inflated_, 0, 0, w, h); + + static_sig_.size_x = static_map.getSizeInCellsX(); + static_sig_.size_y = static_map.getSizeInCellsY(); + static_sig_.resolution = static_map.getResolution(); + static_sig_.origin_x = static_map.getOriginX(); + static_sig_.origin_y = static_map.getOriginY(); + + has_static_inflated_ = true; +} + } // namespace easynav #include diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index 9fc6d254..29433f3b 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,12 +18,10 @@ // along with this program. If not, see . -#include #include #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_costmap_common/costmap_2d.hpp" @@ -40,11 +38,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(NavState & nav_state) @@ -55,22 +51,42 @@ ObstacleFilter::update(NavState & nav_state) const auto & perceptions = nav_state.get("points"); - Costmap2D dynamic_map = nav_state.get("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + Costmap2D & dynamic_map = *dynamic_map_ptr; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + rclcpp::Time stamp; + + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(dynamic_map.getResolution()) + .fuse(tf_info.robot_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(dynamic_map.getResolution()) - .fuse(get_tf_prefix() + "map") - .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .as_points(); + const auto & fused = view.as_points(); + + nav_state.set("map_time", stamp); + + double min_x = std::numeric_limits::max(); + double min_y = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + double max_y = std::numeric_limits::lowest(); for (const auto & p : fused) { + min_x = std::min(min_x, static_cast(p.x)); + min_y = std::min(min_y, static_cast(p.y)); + max_x = std::max(max_x, static_cast(p.x)); + max_y = std::max(max_y, static_cast(p.y)); + unsigned int cx, cy; if (dynamic_map.worldToMap(p.x, p.y, cx, cy)) { dynamic_map.setCost(cx, cy, LETHAL_OBSTACLE); } } - nav_state.set("map.dynamic.filtered", dynamic_map); + if (!fused.empty()) { + ObstacleBounds bb{min_x, min_y, max_x, max_y}; + nav_state.set("map.dynamic.obstacle_bounds", bb); + } } } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index 398e5f63..0d05f16d 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -6,21 +6,11 @@ #include -#include "easynav_costmap_common/costmap_2d.hpp" -#include "easynav_costmap_common/cost_values.hpp" -#include "easynav_costmap_maps_manager/CostmapMapsManager.hpp" -#include "easynav_costmap_maps_manager/map_io.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" +#include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include /// \brief Fixture for CostmapMapsManager tests class CostmapMapsManagerTest : public ::testing::Test @@ -29,17 +19,6 @@ class CostmapMapsManagerTest : public ::testing::Test void SetUp() override { rclcpp::init(0, nullptr); - easynav::NavState::register_printer( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index 09006d4a..ba12d985 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) @@ -90,8 +86,8 @@ ament_export_targets(export_${PROJECT_NAME}) # Register the plugin pluginlib_export_plugin_description_file(easynav_core easynav_navmap_maps_manager_plugins.xml) -pluginlib_export_plugin_description_file(easynav_navmap_filter easynav_navmap_maps_manager_obstacle_filter.xml) -pluginlib_export_plugin_description_file(easynav_navmap_filter easynav_navmap_maps_manager_inflation_filter.xml) +pluginlib_export_plugin_description_file(easynav_navmap_maps_manager easynav_navmap_maps_manager_obstacle_filter.xml) +pluginlib_export_plugin_description_file(easynav_navmap_maps_manager easynav_navmap_maps_manager_inflation_filter.xml) ament_export_dependencies( easynav_common diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 1f7ce673..2f223f28 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -1,66 +1,135 @@ # easynav_navmap_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Maps Manager that maintains a [NavMap](https://github.com/EasyNavigation/NavMap) (triangulated 3D surface) and publishes full maps and layer updates; supports importing from YAML OccupancyGrid or point clouds. +This package also includes map filters implemented as plugins (`ObstacleFilter` and `InflationFilter`) that operate on NavMap layers to detect obstacles and inflate their costs, enabling cost-aware path planning. + ## Authors and Maintainers -- **Authors:** Intelligent Robotics Lab -- **Maintainers:** Francisco Martín Rico + +- **Authors:** Intelligent Robotics Lab +- **Maintainer:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager` -- **Type:** `easynav::NavMapMapsManager` +- **Type:** `easynav::navmap::NavMapMapsManager` - **Base Class:** `easynav::MapsManagerBase` - **Library:** `easynav_navmap_maps_manager` -- **Description:** Maps Manager that maintains a NavMap (triangulated 3D surface) and publishes full maps and layer updates; supports importing from YAML OccupancyGrid or point clouds. +- **Description:** + Maps Manager that maintains a NavMap (triangulated 3D surface) and publishes full maps and layer updates; supports importing from YAML OccupancyGrid or point clouds, and applying dynamic filters to generate obstacle and inflated layers. ## Parameters -All parameters are declared under the plugin namespace, i.e., `//easynav_navmap_maps_manager/NavMapMapsManager/...`. + +All parameters are declared under the plugin namespace, i.e. +`//easynav_navmap_maps_manager/NavMapMapsManager/...` | Name | Type | Default | Description | |---|---|---:|---| -| `.package` | `string` | `""` | Package name to resolve relative paths via ament index. | +| `.package` | `string` | `""` | Package name to resolve relative paths via `ament_index`. | +| `.freq` | `double` | `10.0` | Update frequency (Hz) for map publishing and filter execution. | +| `.navmap_path_file` | `string` | `""` | Relative path (inside the package) to a `.navmap` file to load at startup. | | `.occmap_path_file` | `string` | `""` | Relative path (inside the package) to a ROS YAML OccupancyGrid to import as NavMap. | -| `.navmap_path_file` | `string` | `""` | Relative path (inside the package) to a PCD/PLY used to build NavMap. | +| `.pcd_path_file` | `string` | `""` | Relative path (inside the package) to a point cloud (PCD/PLY) used to build a NavMap surface. | +| `.filters` | `list` | `[]` | Ordered list of filter plugin names to be applied after map loading (e.g. `["obstacles", "inflation"]`). | + +### Filter Plugins + +#### **ObstacleFilter** +- **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/ObstacleFilter` +- **Type:** `easynav::navmap::ObstacleFilter` +- **Description:** + Detects occupied NavCels from input point clouds (`points` key in `NavState`) and marks them as `LETHAL_OBSTACLE` in the `"obstacles"` layer. + The filter groups 3D points into voxels and marks cells as occupied if a sufficient vertical structure is detected (either multiple bins along the z-axis or a vertical span exceeding a threshold). + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| `.vertical_bins_min` | `int` | `3` | Minimum number of vertical bins required to consider a column as an obstacle. | +| `.height_threshold` | `double` | `0.25` | Minimum vertical height (in meters) between max and min z to mark as an obstacle. | +| `.downsample` | `double` | `0.3` | Voxel size used to downsample point clouds before obstacle detection. | +| `.fuse_frame` | `string` | `"map"` | Frame in which points are fused before projection into NavMap. | +| **Input Key:** | | | Reads point clouds from `NavState` key `"points"`. | +| **Output Layer:** | | | Updates or creates NavMap layer `"obstacles"`. | + +#### **InflationFilter** + +- **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter` +- **Type:** `easynav::navmap::InflationFilter` +- **Description:** + Expands obstacle information from the `"obstacles"` layer into an `"inflated_obstacles"` layer, assigning graded costs depending on distance to obstacles and map boundaries (NavCels with missing neighbors). + This filter mimics the behavior of `costmap_2d::InflationLayer` but operates on the NavMap triangular mesh. + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| `.inflation_radius` | `double` | `0.3` | Maximum inflation distance (m) from obstacles. | +| `.cost_scaling_factor` | `double` | `3.0` | Exponential decay rate controlling how fast cost decreases with distance. | +| `.inscribed_radius` | `double` | `0.3` | Radius of inscribed zone (constant high cost before decay). | +| **Input Layer:** | | | Reads from `"obstacles"`. | +| **Output Layer:** | | | Writes to `"inflated_obstacles"`. | ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Publisher | `//map` | `navmap_ros_interfaces/msg/NavMap` | Published full NavMap. | QoS depth=1 | -| Publisher | `//map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | Incremental NavMap layer updates. | QoS depth=100 | -| Subscription | `//incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid to import into NavMap. | QoS depth=1, transient_local, reliable | -| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update NavMap. | QoS depth=100 | - +| Publisher | `//map` | `navmap_ros_interfaces/msg/NavMap` | Publishes the full NavMap. | depth=1 | +| Publisher | `//map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | Publishes incremental layer updates. | depth=100 | +| Subscription | `//incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid to import into NavMap. | depth=1, transient_local, reliable | +| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update NavMap. | depth=100 | ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| -| Service Server | `//savemap` | `std_srvs/srv/Trigger` | Save current NavMap to disk. | - +| Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves the current NavMap and layers to disk. | ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| -| `map` | `::navmap::NavMap` | **Read** | Existing NavMap from state, if available. | -| `map.navmap` | `::navmap::NavMap` | **Write** | Stores/broadcasts the maintained NavMap. | - +| `map.navmap` | `::navmap::NavMap` | **Read** | Reads the NavMap if already present in NavState. | +| `map.navmap` | `::navmap::NavMap` | **Write** | Stores the maintained NavMap, including generated layers `"obstacles"` and `"inflated_obstacles"`. | ## TF Frames + | Role | Transform | Notes | |---|---|---| -| Publishes | `—` | No TF broadcasting in this manager; outputs are stamped in their configured frame. | - +| Publishes | — | No TF broadcasting in this manager; outputs are stamped in their configured frame. | + +## Example Configuration + +```yaml +maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [navmap] + navmap: + freq: 10.0 + plugin: easynav_navmap_maps_manager/NavMapMapsManager + package: easynav_indoor_testcase + navmap_path_file: maps/excavation_urjc.navmap + filters: [obstacles, inflation] + obstacles: + plugin: easynav_navmap_maps_manager/NavMapMapsManager/ObstacleFilter + height_threshold: 0.25 + inflation: + plugin: easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter + inflation_radius: 1.0 + cost_scaling_factor: 2.0 +``` ## License + GPL-3.0-only diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp index c64609bf..6f7c2c1b 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp @@ -24,11 +24,6 @@ #define EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAP_MAPS_MANAGER_HPP_ #include -#include -#include -#include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" @@ -36,17 +31,12 @@ #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "navmap_core/NavMap.hpp" #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" #include "pluginlib/class_loader.hpp" -#include "yaets/tracing.hpp" - namespace easynav { namespace navmap @@ -78,9 +68,9 @@ class NavMapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp index 39881849..74f1725a 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp @@ -21,12 +21,9 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ -#include +#include #include - -#include "pluginlib/class_loader.hpp" - #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -42,8 +39,8 @@ class InflationFilter : public NavMapFilter public: InflationFilter(); - virtual std::expected on_initialize(); - virtual void update(::easynav::NavState & nav_state); + virtual void on_initialize() override; + virtual void update(::easynav::NavState & nav_state) override; bool inflate_layer_u8( ::navmap::NavMap & nm, diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp index 7c57c536..a561ac73 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp @@ -21,11 +21,9 @@ #ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ -#include #include -#include "navmap_core/NavMap.hpp" - +#include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -38,13 +36,12 @@ class NavMapFilter public: NavMapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix = ""); + const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; virtual bool is_adding_layer() {return false;} @@ -53,17 +50,14 @@ class NavMapFilter float get_map_resolution() {return map_resolution_;} void set_map_resolution(float resolution) {map_resolution_ = resolution;} -protected: - std::shared_ptr get_node() const; - const std::string & get_plugin_name() const; - const std::string & get_tf_prefix() const; +protected: + std::shared_ptr get_node() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; float map_resolution_ {0.1}; }; diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp index ebbd3eb4..d4c87015 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp @@ -21,11 +21,8 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#include #include -#include "pluginlib/class_loader.hpp" - #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -41,11 +38,11 @@ class ObstacleFilter : public NavMapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); - virtual void update(::easynav::NavState & nav_state); + virtual void on_initialize() override; + virtual void update(::easynav::NavState & nav_state) override; - bool is_adding_layer() override {return true;} - std::string get_layer_name() override {return "obstacles";} + virtual bool is_adding_layer() override {return true;} + virtual std::string get_layer_name() override {return "obstacles";} private: ::navmap::NavMap navmap_; diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/utils.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/utils.hpp deleted file mode 100644 index 0a69accd..00000000 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/utils.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#ifndef EASYNAV_NAVMAP_MAPS_UTILS_HPP_ -#define EASYNAV_NAVMAP_MAPS_UTILS_HPP_ - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav -{ -namespace navmap -{ - - -inline void triangle_angles_deg( - const Eigen::Vector3f & A, - const Eigen::Vector3f & B, - const Eigen::Vector3f & C, - float & angA, float & angB, float & angC); - -// PCA local para obtener plano tangente en v -// Devuelve dos ejes ortogonales t1, t2 en el plano tangente. -// Si falla PCA (muy pocos puntos), usa un par ortonormal arbitrario. -inline void local_tangent_basis( - const std::vector & nbrs, - Eigen::Vector3f & t1, Eigen::Vector3f & t2); - -// Ordena vecinos por ángulo en el plano tangente de v -inline void sort_neighbors_angular( - const Eigen::Vector3f & vpos, - const std::vector> & nbrs, // (idx, pos) - std::vector & out_ordered); - -// Intenta crear un triángulo (i,j,k) bajo las restricciones -inline bool try_add_triangle( - int i, int j, int k, - const pcl::PointCloud & cloud, - const Params & P, - std::unordered_set & tri_set, - std::unordered_set & edge_set, - std::vector & tris); - - -std::vector grow_surface_from_seed( - const pcl::PointCloud & cloud, - int seed_idx, - const Params & P); -} // namespace navmap -} // namespace easynav -#endif // EASYNAV_NAVMAP_MAPS_UTILS_HPP_ diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index 7725bcc9..c230cb4b 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp @@ -17,14 +17,13 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -45,20 +44,26 @@ NavMapMapsManager::NavMapMapsManager() { ::easynav::NavState::register_printer<::navmap::NavMap>( [](const ::navmap::NavMap & map) { - (void)map; std::ostringstream oss; - oss << "NavMap Navcells = " << map.navcels.size() << "\tlayers = " << - map.list_layers().size(); + oss << "NavMap: navcels=" << map.navcels.size() + << " surfaces=" << map.surfaces.size() + << " layers=" << map.list_layers().size() << " ("; + + for (const auto & layer : map.list_layers()) { + oss << layer << " "; + } + oss << ")"; + return oss.str(); }); - // navmap_filters_loader_ = std::make_unique>( - // "easynav_navmap_filter", "easynav::navmap::NavMapFilter"); + navmap_filters_loader_ = std::make_unique>( + "easynav_navmap_maps_manager", "easynav::navmap::NavMapFilter"); } NavMapMapsManager::~NavMapMapsManager() {} -std::expected +void NavMapMapsManager::on_initialize() { auto node = get_node(); @@ -85,18 +90,15 @@ NavMapMapsManager::on_initialize() try { RCLCPP_INFO(node->get_logger(), "Loading NavMapFilter %s [%s]", navmap_filter.c_str(), plugin.c_str()); - std::shared_ptr instance; instance = navmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + navmap_filter, - get_tf_prefix()); - - if (!result) { + try { + instance->initialize(node, plugin_name + "." + navmap_filter); + } catch (const std::runtime_error & ex) { RCLCPP_ERROR(node->get_logger(), - "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - return std::unexpected("Unable to initialize " + - plugin + " . Error: " + result.error()); + "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); + throw; } navmap_filters_.push_back(instance); @@ -106,7 +108,7 @@ NavMapMapsManager::on_initialize() } catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR(node->get_logger(), "Unable to load plugin easynav::navmap::NavMapFilter. Error: %s", ex.what()); - return std::unexpected("Unable to load plugin easynav::navmap::NavMapFilter " + + throw std::runtime_error("Unable to load plugin easynav::navmap::NavMapFilter " + navmap_filter + " . Error: " + ex.what()); } } @@ -119,25 +121,28 @@ NavMapMapsManager::on_initialize() node->get_fully_qualified_name() + std::string("/") + plugin_name + "/map_updates", rclcpp::QoS(100)); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + if (!package_name.empty() && !occmap_path_file.empty()) { try { const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } nav_msgs::msg::OccupancyGrid occ_msg; if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } resolution_ = occ_msg.info.resolution; navmap_ = navmap_ros::from_occupancy_grid(occ_msg); navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } @@ -147,12 +152,12 @@ NavMapMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + navmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (navmap_ros::io::load_from_file(map_path_, navmap_)) { navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } else { @@ -163,13 +168,13 @@ NavMapMapsManager::on_initialize() incoming_occ_map_sub_ = node->create_subscription( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), - [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { + [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { resolution_ = msg->info.resolution; navmap_ = navmap_ros::from_occupancy_grid(*msg); navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); @@ -177,13 +182,13 @@ NavMapMapsManager::on_initialize() incoming_pc2_map_sub_ = node->create_subscription( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), - [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { + [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { navmap_ros::BuildParams params; navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); @@ -200,8 +205,6 @@ NavMapMapsManager::on_initialize() navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); // ToDo }); - - return {}; } void @@ -219,27 +222,17 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) nav_state.set("map.navmap", navmap_); } - nav_state.set("map.navmap", navmap_); + for (const auto & filter : navmap_filters_) { + filter->set_map_resolution(resolution_); + filter->update(nav_state); -// if (!navmap_.has_layer("occupancy")) { -// navmap_.add_layer("occupancy", "Per-NavCel occupancy (0=free, 255=unknown)", "", -// static_cast(0)); -// } -// -// for (const auto & filter : navmap_filters_) { -// filter->set_map_resolution(resolution_); -// filter->update(nav_state); -// } -// -// navmap_ = nav_state.get<::navmap::NavMap>("map"); -// -// for (const auto & filter : navmap_filters_) { -// -// if (filter->is_adding_layer() && navmap_.has_layer(filter->get_layer_name())) { -// auto update_msg = navmap_ros::to_msg(navmap_, filter->get_layer_name()); -// layer_updates_pub_->publish(update_msg); -// } -// } + const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); + + if (filter->is_adding_layer() && navmap.has_layer(filter->get_layer_name())) { + auto update_msg = navmap_ros::to_msg(navmap, filter->get_layer_name()); + layer_updates_pub_->publish(update_msg); + } + } } diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 4935d8bb..33d6c950 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp @@ -39,15 +39,13 @@ *********************************************************************/ -#include #include #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/InflationFilter.hpp" @@ -57,17 +55,11 @@ namespace easynav namespace navmap { -static constexpr unsigned char NO_INFORMATION = 255; -static constexpr unsigned char LETHAL_OBSTACLE = 254; -static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; -static constexpr unsigned char MAX_NON_OBSTACLE = 252; -static constexpr unsigned char FREE_SPACE = 0; - InflationFilter::InflationFilter() -: inflation_radius_(0), - cost_scaling_factor_(0) -{ -} +: inflation_radius_(0.0f), + cost_scaling_factor_(0.0f), + inscribed_radius_(0.0f) +{} bool InflationFilter::inflate_layer_u8( ::navmap::NavMap & nm, @@ -77,94 +69,118 @@ bool InflationFilter::inflate_layer_u8( float cost_scaling_factor, float inscribed_radius) { - using namespace ::navmap; + using ::navmap::NavCelId; + using namespace navmap_ros; // bring FREE_SPACE, LETHAL_OBSTACLE, etc. + if (nm.navcels.empty() || inflation_radius <= 0.0f || cost_scaling_factor <= 0.0f) { return false; } - // Capas - auto src_view = std::dynamic_pointer_cast>(nm.layers.get(src_layer)); + // Source layer + auto src_view = + std::dynamic_pointer_cast<::navmap::LayerView>(nm.layers.get(src_layer)); if (!src_view || src_view->size() != nm.navcels.size()) { return false; } - auto dst_view = nm.layers.add_or_get(dst_layer, nm.navcels.size(), - layer_type_tag()); + + // Destination layer (create if missing) + auto dst_view = nm.layers.add_or_get(dst_layer, nm.navcels.size(), + ::navmap::layer_type_tag()); if (!dst_view) {return false;} if (dst_view->data().size() != nm.navcels.size()) { - const_cast &>(dst_view->data()).assign(nm.navcels.size(), FREE_SPACE); + const_cast &>(dst_view->data()).assign(nm.navcels.size(), FREE_SPACE); } const size_t N = nm.navcels.size(); - const float R = inflation_radius; - // const float k = cost_scaling_factor; - const float r_ins = std::clamp(inscribed_radius, 0.0f, R); + const float R = inflation_radius; + const float r_ins = std::clamp(inscribed_radius, 0.0f, R); - // Precomputar centroides XY + // Precompute XY centroids for each NavCel std::vector C(N); for (NavCelId cid = 0; cid < N; ++cid) { const Eigen::Vector3f cc = nm.navcel_centroid(cid); C[cid] = {cc.x(), cc.y()}; } - // Distancias y cola (Dijkstra multi-fuente) + // Distance map (multi-source Dijkstra) const float INF = std::numeric_limits::infinity(); std::vector dist(N, INF); struct Node { float d; NavCelId cid; bool operator<(const Node & o) const {return d > o.d;} }; std::priority_queue pq; - // Inicializar dst y semillas - auto & dst = dst_view->mutable_data(); // marca dirty + auto & dst = dst_view->mutable_data(); // mark dirty const auto & src = src_view->data(); + // Initialize destinations and seeds bool any_seed = false; for (NavCelId cid = 0; cid < N; ++cid) { - const uint8_t s = src[cid]; + const std::uint8_t s = src[cid]; + + // Seed 1: lethal obstacles if (s == LETHAL_OBSTACLE) { dist[cid] = 0.0f; pq.push({0.0f, cid}); dst[cid] = LETHAL_OBSTACLE; any_seed = true; - } else if (s == NO_INFORMATION) { - dst[cid] = NO_INFORMATION; // mantener desconocido + continue; + } + + // Seed 2: boundary triangles (missing neighbors) + if (nm.navcel_neighbors(cid).size() < 3) { + dist[cid] = 0.0f; + pq.push({0.0f, cid}); + dst[cid] = LETHAL_OBSTACLE; + any_seed = true; + continue; + } + + // Unknown handling + if (s == NO_INFORMATION) { + dst[cid] = NO_INFORMATION; } else { - // si dst==src (in-place), preserva el valor actual; si no, escribe 0 - if (dst_layer != src_layer) {dst[cid] = FREE_SPACE;} + if (dst_layer != src_layer) { + dst[cid] = FREE_SPACE; + } } } - if (!any_seed) {return true;} // nada que inflar - auto cost_from_dist = [&](float d) -> uint8_t { - if (d <= 0.0f) {return LETHAL_OBSTACLE;} // 254 - if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} // 253 - if (d > R) {return FREE_SPACE;} // 0 + if (!any_seed) { + return true; // nothing to inflate + } + + // Distance-to-cost conversion + auto cost_from_dist = [&](float d) -> std::uint8_t { + if (d <= 0.0f) {return LETHAL_OBSTACLE;} + if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} + if (d > R) {return FREE_SPACE;} - const float x = d - r_ins; // >= 0 - const double factor = -1.0 * cost_scaling_factor * x; - double c = std::exp(factor) * (static_cast(INSCRIBED_INFLATED_OBSTACLE) - 1); + const float x = d - r_ins; + const double base = static_cast(INSCRIBED_INFLATED_OBSTACLE) - 1; + double c = std::exp(-cost_scaling_factor * static_cast(x)) * base; if (c < 0.0) {c = 0.0;} - if (c > INSCRIBED_INFLATED_OBSTACLE - 1) {c = INSCRIBED_INFLATED_OBSTACLE - 1;} - return static_cast(std::lround(c)); + if (c > base) {c = base;} + return static_cast(std::lround(c)); }; - // Dijkstra acotado por R + // Bounded Dijkstra expansion while (!pq.empty()) { const auto [du, u] = pq.top(); pq.pop(); if (du != dist[u]) {continue;} if (du > R) {continue;} - // Escribir coste si no es desconocido ni letal if (dst[u] != NO_INFORMATION && dst[u] != LETHAL_OBSTACLE) { - const uint8_t c = cost_from_dist(du); - if (c > dst[u]) {dst[u] = c;} // tomamos el máximo (monótono) + const std::uint8_t c = cost_from_dist(du); + if (c > dst[u]) {dst[u] = c;} // monotonic accumulation } - // Relajar vecinos + // Neighbor relaxation for (NavCelId v : nm.navcel_neighbors(u)) { const size_t vidx = static_cast(v); if (vidx >= N) {continue;} - // Si quieres que el desconocido BLOQUEE la propagación, descomenta: - // if (src[v] == NO_INFORMATION) continue; + if (src[v] == NO_INFORMATION) { + continue; + } const float step = (C[u] - C[v]).norm(); if (step <= 0.0f) {continue;} @@ -179,49 +195,55 @@ bool InflationFilter::inflate_layer_u8( return true; } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); - inflation_radius_ = 0.3; - cost_scaling_factor_ = 3.0; - inscribed_radius_ = 0.3; + // Defaults; may be overridden in parameters + inflation_radius_ = 0.30f; + cost_scaling_factor_ = 3.0f; + inscribed_radius_ = 0.30f; node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); + node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->get_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); RCLCPP_INFO(node->get_logger(), - "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", - inflation_radius_, cost_scaling_factor_); - - return {}; + "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f", + inflation_radius_, cost_scaling_factor_, inscribed_radius_); } -void -InflationFilter::update(::easynav::NavState & nav_state) +void InflationFilter::update(::easynav::NavState & nav_state) { - if (!nav_state.has("map")) { + if (!nav_state.has("map.navmap")) { return; } - navmap_ = nav_state.get<::navmap::NavMap>("map"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + + const bool ok = inflate_layer_u8( + navmap_, + "obstacles", + "inflated_obstacles", + inflation_radius_, + cost_scaling_factor_, + inscribed_radius_); - if (!inflate_layer_u8(navmap_, "obstacles", "inflated_obstacles", - inflation_radius_, cost_scaling_factor_, 0.3)) - { - RCLCPP_ERROR(parent_node_->get_logger(), "Error inflating at ObstacleFilter"); + if (!ok) { + RCLCPP_ERROR(parent_node_->get_logger(), "InflationFilter: inflate_layer_u8() failed"); + return; } - nav_state.set("map", navmap_); + nav_state.set("map.navmap", navmap_); } - } // namespace navmap } // namespace easynav + #include PLUGINLIB_EXPORT_CLASS(easynav::navmap::InflationFilter, easynav::navmap::NavMapFilter) diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp index a850edd7..139278ba 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp @@ -18,12 +18,8 @@ // along with this program. If not, see . -#include #include -#include "navmap_core/NavMap.hpp" -#include "easynav_common/types/NavState.hpp" - #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" namespace easynav @@ -36,18 +32,15 @@ NavMapFilter::NavMapFilter() } -std::expected +void NavMapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix -) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; - return on_initialize(); + on_initialize(); } std::shared_ptr @@ -62,11 +55,5 @@ NavMapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -NavMapFilter::get_tf_prefix() const -{ - return tf_prefix_; -} - } // namespace navmap } // namespace easynav diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index ffd0838f..cddda7ba 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,14 +18,15 @@ // along with this program. If not, see . -#include #include +#include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/ObstacleFilter.hpp" @@ -40,54 +41,126 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} -void -ObstacleFilter::update(::easynav::NavState & nav_state) +void ObstacleFilter::update(::easynav::NavState & nav_state) { - if (!nav_state.has("map")) { - return; - } - if (!nav_state.has("points")) { - return; - } + if (!nav_state.has("map.navmap")) {return;} + if (!nav_state.has("points")) {return;} const auto & perceptions = nav_state.get("points"); - navmap_ = nav_state.get<::navmap::NavMap>("map"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (!navmap_.layer_copy("occupancy", "obstacles")) { - RCLCPP_ERROR(parent_node_->get_logger(), "Error copying layers at ObstacleFilter"); - return; - } + navmap_.layer_clear(get_layer_name(), navmap_ros::FREE_SPACE); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(get_map_resolution()) - .fuse(get_tf_prefix() + "map") - .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + const auto & points = PointPerceptionsOpsView(perceptions) + .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) + .downsample(0.3) + .fuse(tf_info.map_frame) .as_points(); - size_t sidx = 0; - for (const auto & p : fused) { - if (std::isnan(p.x)) {continue;} - ::navmap::NavCelId cid; - Eigen::Vector3f bary; - Eigen::Vector3f hit; + const float voxel_xy = 0.30f; + const float voxel_z = 0.20f; + + struct Accum + { + std::unordered_set z_bins; + float max_z = -std::numeric_limits::infinity(); + float min_z = std::numeric_limits::infinity(); + }; + + struct Key + { + int ix, iy; + bool operator==(const Key & o) const noexcept {return ix == o.ix && iy == o.iy;} + }; + struct KeyHash + { + std::size_t operator()(const Key & k) const noexcept + { + std::size_t h1 = std::hash{}(static_cast(k.ix)); + std::size_t h2 = std::hash{}(static_cast(k.iy)); + return h1 ^ (h2 + 0x9e3779b97f4a7c15ULL + (h1 << 6) + (h1 >> 2)); + } + }; + + std::unordered_map bins; + bins.reserve(points.size() / 4 + 1); + + for (const auto & p : points.points) { + const float x = p.x; + const float y = p.y; + const float z = p.z; + + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) {continue;} + + const int ix = static_cast(std::floor(x / voxel_xy)); + const int iy = static_cast(std::floor(y / voxel_xy)); + const int iz = static_cast(std::floor(z / voxel_z)); - if (navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit)) { - uint8_t v = navmap_.layer_get("occupancy", cid, 255); - if (v == 0) { - navmap_.set_area(hit, 254, "obstacles", ::navmap::AreaShape::CIRCULAR, 0.1); + auto & acc = bins[{ix, iy}]; + acc.z_bins.insert(iz); + if (z > acc.max_z) {acc.max_z = z;} + if (z < acc.min_z) {acc.min_z = z;} + } + + std::optional last_surface; + std::optional<::navmap::NavCelId> last_cid; + + const float height_threshold = 0.25f; + + for (const auto & kv : bins) { + const auto & key = kv.first; + const auto & acc = kv.second; + + const float cx = (static_cast(key.ix) + 0.5f) * voxel_xy; + const float cy = (static_cast(key.iy) + 0.5f) * voxel_xy; + const float dz = acc.max_z - acc.min_z; + + if (acc.z_bins.size() <= 2 && dz <= height_threshold) { + continue; + } + + const float cz = acc.max_z; + Eigen::Vector3f query(cx, cy, cz); + + size_t surface_idx = 0; + ::navmap::NavCelId cid; + Eigen::Vector3f bary, hit; + + ::navmap::NavMap::LocateOpts opts; + opts.use_downward_ray = true; + opts.height_eps = 0.50f; + if (last_surface) {opts.hint_surface = *last_surface;} + if (last_cid) {opts.hint_cid = *last_cid;} + + bool ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit, opts); + + if (!ok) { + ::navmap::NavMap::LocateOpts nohint; + nohint.use_downward_ray = true; + nohint.height_eps = 0.50f; + ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit, nohint); + if (!ok) { + ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit); } } + + if (ok) { + navmap_.layer_set( + get_layer_name(), cid, static_cast(navmap_ros::LETHAL_OBSTACLE)); + last_surface = surface_idx; + last_cid = cid; + } } - nav_state.set("map", navmap_); + nav_state.set("map.navmap", navmap_); } + } // namespace navmap } // namespace easynav #include diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/utils.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/utils.cpp deleted file mode 100644 index 30b07dc3..00000000 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/utils.cpp +++ /dev/null @@ -1,204 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#include "easynav_navmap_maps_manager/utils.hpp" - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav -{ -namespace navmap -{ - -// PCA local para obtener plano tangente en v -// Devuelve dos ejes ortogonales t1, t2 en el plano tangente. -// Si falla PCA (muy pocos puntos), usa un par ortonormal arbitrario. -inline void local_tangent_basis( - const std::vector & nbrs, - Eigen::Vector3f & t1, Eigen::Vector3f & t2) -{ - if (nbrs.size() < 3) { - // base por defecto (proyección estable) - t1 = Eigen::Vector3f::UnitX(); - t2 = Eigen::Vector3f::UnitY(); - return; - } - - // Centra - Eigen::Vector3f mean = Eigen::Vector3f::Zero(); - for (auto & p : nbrs) { - mean += p; - } - mean /= static_cast(nbrs.size()); - - // Covarianza - Eigen::Matrix3f C = Eigen::Matrix3f::Zero(); - for (auto & p : nbrs) { - Eigen::Vector3f d = p - mean; - C += d * d.transpose(); - } - C /= static_cast(nbrs.size()); - - // Autovectores - Eigen::SelfAdjointEigenSolver es(C); - // Valores ascendentes: 0 -> menor (normal aproximada) - // Eigen::Vector3f e0 = es.eigenvectors().col(0); // normal aproximada - Eigen::Vector3f e1 = es.eigenvectors().col(1); - Eigen::Vector3f e2 = es.eigenvectors().col(2); - - // Plano tangente: proyectaremos en {e1, e2} - // Asegura ortonormalidad y evita inversión esporádica - t1 = e1.normalized(); - t2 = (e2 - e2.dot(t1) * t1).normalized(); -} - -inline void triangle_angles_deg( - const Eigen::Vector3f & A, - const Eigen::Vector3f & B, - const Eigen::Vector3f & C, - float & angA, float & angB, float & angC) -{ - // Lados opuestos a A, B, C - float a = (B - C).norm(); - float b = (C - A).norm(); - float c = (A - B).norm(); - - // Evitar divisiones por cero - const float eps = 1e-12f; - a = std::max(a, eps); - b = std::max(b, eps); - c = std::max(c, eps); - - // Ley de cosenos con clamp numérico - auto angle_from = [](float opp, float x, float y) -> float { - float cosv = (x * x + y * y - opp * opp) / (2.0f * x * y); - cosv = std::min(1.0f, std::max(-1.0f, cosv)); - return std::acos(cosv) * 180.0f / static_cast(M_PI); - }; - - angA = angle_from(a, b, c); - angB = angle_from(b, c, a); - angC = angle_from(c, a, b); -} - -// Ordena vecinos por ángulo en el plano tangente de v -inline void sort_neighbors_angular( - const Eigen::Vector3f & vpos, - const std::vector> & nbrs, // (idx, pos) - std::vector & out_ordered) -{ - std::vector pts; - pts.reserve(nbrs.size() + 1); - pts.push_back(vpos); - for (auto & it : nbrs) { - pts.push_back(it.second); - } - - Eigen::Vector3f t1, t2; - local_tangent_basis(pts, t1, t2); - - std::vector> ang_idx; - ang_idx.reserve(nbrs.size()); - for (auto & it : nbrs) { - Eigen::Vector3f d = it.second - vpos; - float x = d.dot(t1); - float y = d.dot(t2); - float ang = std::atan2(y, x); // -pi..pi - ang_idx.emplace_back(ang, it.first); - } - - std::sort(ang_idx.begin(), ang_idx.end(), - [](auto & a, auto & b){return a.first < b.first;}); - - out_ordered.clear(); - out_ordered.reserve(ang_idx.size()); - for (auto & ai : ang_idx) { - out_ordered.push_back(ai.second); - } -} - -// Intenta crear un triángulo (i,j,k) bajo las restricciones -inline bool try_add_triangle( - int i, int j, int k, - const pcl::PointCloud & cloud, - const Params & P, - std::unordered_set & tri_set, - std::unordered_set & edge_set, - std::vector & tris) -{ - TriKey tk = make_tri(i, j, k); - if (tri_set.find(tk) != tri_set.end()) {return false;} - - const auto & A = cloud[i]; - const auto & B = cloud[j]; - const auto & C = cloud[k]; - if (!pcl::isFinite(A) || !pcl::isFinite(B) || !pcl::isFinite(C)) {return false;} - - // Longitudes máximas de arista - auto dAB = dist3(A, B); - auto dBC = dist3(B, C); - auto dCA = dist3(C, A); - if (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len) {return false;} - - Eigen::Vector3f a(A.x, A.y, A.z); - Eigen::Vector3f b(B.x, B.y, B.z); - Eigen::Vector3f c(C.x, C.y, C.z); - - // Área mínima - if (tri_area(a, b, c) < P.min_area) {return false;} - - // Pendiente máxima - float slope = tri_slope_deg(a, b, c); - if (slope > P.max_slope_deg) {return false;} - - // Ángulo mínimo en los tres vértices - float angA, angB, angC; - triangle_angles_deg(a, b, c, angA, angB, angC); - if (angA < P.min_angle_deg || angB < P.min_angle_deg || angC < P.min_angle_deg) { - return false; - } - - { - const Eigen::Vector3f up = Eigen::Vector3f::UnitZ(); - Eigen::Vector3f n = (b - a).cross(c - a); - if (n.norm() < 1e-9f) {return false;} - if (n.dot(up) < 0.0f) { - std::swap(j, k); - // si más abajo usas b/c otra vez, re-asigna: - // b = Eigen::Vector3f(cloud[j].x, cloud[j].y, cloud[j].z); - // c = Eigen::Vector3f(cloud[k].x, cloud[k].y, cloud[k].z); - } - } - - // Aceptar - tri_set.insert(tk); - edge_set.insert(make_edge(i, j)); - edge_set.insert(make_edge(j, k)); - edge_set.insert(make_edge(k, i)); - tris.emplace_back(Triangle(i, j, k)); - return true; -} - - -} // namespace navmap -} // namespace easynav diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index c4e1438f..9de01f7e 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp @@ -6,21 +6,14 @@ #include -#include "navmap_core/NavMap.hpp" - -#include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" -#include "easynav_navmap_maps_manager/map_io.hpp" - -#include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" +#include "easynav_common/types/NavState.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_srvs/srv/trigger.hpp" #include -#include /// \brief Fixture for NavMapMapsManager tests class NavMapMapsManagerTest : public ::testing::Test @@ -29,17 +22,6 @@ class NavMapMapsManagerTest : public ::testing::Test void SetUp() override { rclcpp::init(0, nullptr); - ::easynav::NavState::register_printer( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override diff --git a/maps_managers/easynav_navmap_maps_manager/tests/test_utils.cpp b/maps_managers/easynav_navmap_maps_manager/tests/test_utils.cpp deleted file mode 100644 index 5d17909b..00000000 --- a/maps_managers/easynav_navmap_maps_manager/tests/test_utils.cpp +++ /dev/null @@ -1,205 +0,0 @@ -// Copyright 2025 Intelligent Robotics Lab -// -// This file is part of the project Easy Navigation (EasyNav in short) -// licensed under the GNU General Public License v3.0. -// See for details. - -#include - -#include "easynav_navmap_maps_manager/utils.hpp" - -/// \brief Fixture for NavMapMapsManager tests -class NavMapMapsManagerUtilsTests : public ::testing::Test -{ -protected: - void SetUp() override - { - } - - void TearDown() override - { - } -}; - -TEST_F(NavMapMapsManagerUtilsTests, BasicUsage) -{ - easynav::navmap::Params P; - P.max_edge_len = 2.4f; // algo > resolución del downsample - P.max_slope_deg = 25.0f; // ajusta según “transitabilidad” - P.neighbor_radius = 2.4f; // radio razonable - P.use_radius = true; - - pcl::PointCloud input_points; - - input_points.push_back({0.5, 0.5, 0.0}); - input_points.push_back({1.9, 0.5, 0.0}); - input_points.push_back({0.5, 1.5, 0.0}); - input_points.push_back({-0.5, 1.5, 0.0}); - - int seed = 0; - auto triangles = easynav::navmap::grow_surface_from_seed(input_points, seed, P); - - - std::cerr << "Triángulos: " << triangles.size() << std::endl; - for (size_t i = 0; i < triangles.size(); i++) { - pcl::PointXYZ v0 = input_points[triangles[i][0]]; - pcl::PointXYZ v1 = input_points[triangles[i][1]]; - pcl::PointXYZ v2 = input_points[triangles[i][2]]; - - - std::cerr << i << "\t(" << v0.x << ", " << v0.y << ", " << v0.z << ")" << - "\t(" << v1.x << ", " << v1.y << ", " << v1.z << ")" << - "\t(" << v2.x << ", " << v2.y << ", " << v2.z << ")" << std::endl; - } - -} - - -///// \brief Dynamic map update test with point cloud -//TEST_F(NavMapMapsManagerTest, BasicDynamicUpdate) -//{ -// auto node = std::make_shared("test_node"); -// auto manager = std::make_shared(); -// manager->initialize(node, "test"); -// -// easynav::NavMap2D static_map(30, 30, 0.1, -1.5, -1.5); -// manager->set_static_map(static_map); -// -// ::easynav::NavState ::easynav::NavState; -// auto perception = std::make_shared(); -// perception->data.points.resize(2); -// perception->data.points[0].x = 1.0; -// perception->data.points[0].y = 1.0; -// perception->data.points[0].z = 0.2; -// perception->data.points[1].x = -1.0; -// perception->data.points[1].y = -1.0; -// perception->data.points[1].z = 0.2; -// perception->frame_id = "map"; -// perception->stamp = node->now(); -// perception->valid = true; -// -// easynav::PointPerceptions perceptions; -// perceptions.push_back(perception); -// ::easynav::NavState.set("points", perceptions); -// -// manager->update(::easynav::NavState); -// -// ASSERT_TRUE(::easynav::NavState.has("map.dynamic")); -// const auto & map = ::easynav::NavState.get("map.dynamic"); -// -// unsigned int cx, cy; -// ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); -// EXPECT_EQ(map.getCost(cx, cy), easynav::LETHAL_OBSTACLE); -// -// ASSERT_TRUE(map.worldToMap(-1.0, -1.0, cx, cy)); -// EXPECT_EQ(map.getCost(cx, cy), easynav::LETHAL_OBSTACLE); -//} -// -///// \brief OccupancyGrid updates map via subscription -//TEST_F(NavMapMapsManagerTest, IncomingOccupancyGridUpdatesMaps) -//{ -// auto node = std::make_shared("test_node2"); -// auto manager = std::make_shared(); -// manager->initialize(node, "test2"); -// -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(node->get_node_base_interface()); -// -// auto pub = node->create_publisher( -// "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); -// -// pub->on_activate(); -// -// nav_msgs::msg::OccupancyGrid grid; -// grid.header.frame_id = "map"; -// grid.info.width = 10; -// grid.info.height = 10; -// grid.info.resolution = 0.2; -// grid.info.origin.position.x = -1.0; -// grid.info.origin.position.y = -0.6; -// grid.data.assign(100, 0); -// grid.data[55] = 100; -// -// pub->publish(grid); -// executor.spin_some(); -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// -// ::easynav::NavState ::easynav::NavState; -// manager->update(::easynav::NavState); -// -// ASSERT_TRUE(::easynav::NavState.has("map.static")); -// const auto & map = ::easynav::NavState.get("map.static"); -// -// EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); -// EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); -//} -// -///// \brief Helper subclass to force map path for savemap -//class FriendNavMapMapsManager : public easynav::navmap::NavMapMapsManager -//{ -//public: -// void force_path(const std::string & path) {map_path_ = path;} -//}; -// -///// \brief Test that the savemap service correctly stores YAML and PGM files -//TEST_F(NavMapMapsManagerTest, SavemapServiceWorks) -//{ -// auto node = std::make_shared("test_savemap_node"); -// auto manager = std::make_shared(); -// manager->initialize(node, "test_savemap"); -// -// // Create a 200x200 static ::navmap::NavMap with resolution 0.05 and origin at (0, 0) -// const unsigned int width = 200; -// const unsigned int height = 200; -// easynav::NavMap2D map_static(width, height, 0.05, 0.0, 0.0); -// -// // Set a vertical line of lethal obstacles at x = 30 -// for (unsigned int y = 0; y < height; ++y) { -// map_static.setCost(30, y, easynav::LETHAL_OBSTACLE); -// } -// -// manager->set_static_map(map_static); -// -// const std::string yaml_path = "/tmp/savemap_test_map"; -// const std::string service_name = "/test_savemap_node/test_savemap/savemap"; -// -// manager->force_path(yaml_path); -// -// // Create executor and service client -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(node->get_node_base_interface()); -// -// auto client = node->create_client(service_name); -// ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); -// -// // Call savemap service -// auto request = std::make_shared(); -// auto future = client->async_send_request(request); -// executor.spin_until_future_complete(future); -// -// auto response = future.get(); -// EXPECT_TRUE(response->success); -// EXPECT_NE(response->message.find("saved"), std::string::npos); -// -// // Reload the map from the generated YAML + PGM files -// nav_msgs::msg::OccupancyGrid loaded_grid; -// EXPECT_EQ(easynav::loadMapFromYaml(yaml_path + ".yaml", loaded_grid), easynav::LOAD_MAP_SUCCESS); -// -// easynav::NavMap2D loaded_map(loaded_grid); -// -// // Check map dimensions match -// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_static.getSizeInCellsX()); -// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_static.getSizeInCellsY()); -// -// for (unsigned int y = 0; y < map_static.getSizeInCellsY(); ++y) { -// for (unsigned int x = 0; x < map_static.getSizeInCellsX(); ++x) { -// if (x == 30) { -// EXPECT_EQ(loaded_map.getCost(x, y), easynav::LETHAL_OBSTACLE) -// << "Expected LETHAL_OBSTACLE at (" << x << "," << y << ")"; -// } else { -// EXPECT_EQ(loaded_map.getCost(x, y), easynav::FREE_SPACE) -// << "Expected FREE_SPACE at (" << x << "," << y << ")"; -// } -// } -// } -//} diff --git a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt index ea292b81..fc7b4b24 100644 --- a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) diff --git a/maps_managers/easynav_octomap_maps_manager/README.md b/maps_managers/easynav_octomap_maps_manager/README.md index 700ce944..5d60468d 100644 --- a/maps_managers/easynav_octomap_maps_manager/README.md +++ b/maps_managers/easynav_octomap_maps_manager/README.md @@ -1,21 +1,23 @@ # easynav_octomap_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Maps Manager that maintains an [OctoMap](https://octomap.github.io/) (probabilistic 3D occupancy tree), supports filter plugins (e.g., inflation and obstacle filters), and exposes the map through ROS 2 topics. This README documents **plugin-level parameters only**. ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_octomap_maps_manager/OctomapMapsManager` - **Type:** `easynav::octomap::OctomapMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -26,19 +28,16 @@ Maps Manager that maintains an [OctoMap](https://octomap.github.io/) (probabilis ## Parameters -### Plugin Parameters (namespace: `//easynav_octomap_maps_manager/OctomapMapsManager/...`) -| Name | Type | Default | Description | -|---|---|---:|---| -| `.package` | `string` | `""` | Package name used to resolve relative map paths via `ament_index`. | -| `.occmap_path_file` | `string` | `""` | Relative path (inside the package) to a ROS YAML occupancy grid to import as OctoMap. | -| `.octomap_path_file` | `string` | `""` | Relative path (inside the package) to a PCD/PLY/OT file used to build/load the OctoMap. | -| `.filters` | `string[]` | `[]` | List of filter identifiers to be instantiated (see “Filter Parameters”). | -| `..plugin` | `string` | `""` | Type of filter plugin (e.g., `easynav_octomap_maps_manager/InflationFilter`). | +### Plugin Parameters + +The current code (OctomapMapsManager.cpp) has the high-level parameter declarations for package and map paths commented out. Active runtime parameters are therefore limited to filter plugin namespaces populated by entries in `filters` (if/when enabled). Until the declarations are restored, only filter-local parameters are honored. ### Filter Parameters + Each entry in `.filters` defines a sub-namespace `.` with at least the key `plugin`, plus any specific parameters. #### InflationFilter + | Name | Type | Default | Description | |---|---|---:|---| | `.inflation.inflation_radius` | `double` | `0.3` | Inflation radius (m) used to expand occupied cells. | @@ -46,9 +45,10 @@ Each entry in `.filters` defines a sub-namespace `.` wit | `.inflation.inscribed_radius` | `double` | `0.3` | Inscribed radius (m) used by the inflation model. | #### ObstacleFilter + This filter does not declare additional ROS parameters apart from `plugin`. -**Example Configuration** +## Example Configuration ```yaml maps_manager_node: @@ -73,12 +73,14 @@ maps_manager_node: ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| -| Publisher | `//map` | `octomap_msgs/msg/Octomap` | Publishes the current OctoMap. | `depth=1` | -| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud used to build/update the OctoMap. | `depth=100` | +| Publisher | `//map` | `octomap_msgs/msg/Octomap` | Publishes the current OctoMap. | depth=1 | +| Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud used to build/update the OctoMap. | depth=100 | ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves the current OctoMap to disk. | @@ -86,13 +88,15 @@ maps_manager_node: --- ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| -| `map` | `::octomap::Octomap` | **Read** | If present in NavState, used as input/seed map. This plugin does not write a NavState key. | +| `map` | `::octomap::Octomap` | **Read** | If present in NavState, used as an input/seed map. (Plugin currently does not write back to NavState.) | --- ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | — | This manager does not broadcast TF; outputs are stamped using the configured frame(s) in the map data. | @@ -100,4 +104,5 @@ maps_manager_node: --- ## License + GPL-3.0-only diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp index 4654a14d..8b4f79ee 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp @@ -24,28 +24,18 @@ #define EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAP_MAPS_MANAGER_HPP_ #include -#include -#include -#include -#include -#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "octomap_msgs/msg/octomap.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "octomap/octomap.h" #include "easynav_octomap_maps_manager/filters/OctomapFilter.hpp" #include "pluginlib/class_loader.hpp" -#include "yaets/tracing.hpp" - namespace easynav { namespace octomap @@ -77,9 +67,9 @@ class OctomapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp index a2d42a7e..c0524721 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp @@ -21,10 +21,8 @@ #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ -#include #include - #include "pluginlib/class_loader.hpp" #include "octomap_core/Octomap.hpp" @@ -42,7 +40,7 @@ class InflationFilter : public OctomapFilter public: InflationFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(::easynav::NavState & nav_state); bool inflate_layer_u8( diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp index 91d57b7f..571433c3 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp @@ -21,7 +21,6 @@ #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#include #include #include "pluginlib/class_loader.hpp" @@ -41,7 +40,7 @@ class ObstacleFilter : public OctomapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(::easynav::NavState & nav_state); bool is_adding_layer() override {return true;} diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp index fd6d4df0..efa8a984 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp @@ -21,11 +21,10 @@ #ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ -#include #include #include "octomap/octomap.h" - +#include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -38,13 +37,12 @@ class OctomapFilter public: OctomapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix = ""); + const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; float get_map_resolution() {return map_resolution_;} @@ -55,12 +53,9 @@ class OctomapFilter const std::string & get_plugin_name() const; - const std::string & get_tf_prefix() const; - protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; float map_resolution_ {0.1}; }; diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/utils.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/utils.hpp deleted file mode 100644 index ca22b0fb..00000000 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/utils.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* OccupancyGrid map input-output library */ - -#ifndef EASYNAV_OCTOMAP_MAPS_MANAGER_UTILS_HPP_ -#define EASYNAV_OCTOMAP_MAPS_MANAGER_UTILS_HPP_ - -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" - -#include "pcl/common/transforms.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl/PointIndices.h" -#include "pcl/kdtree/kdtree_flann.h" - -namespace easynav -{ -namespace octomap -{ - - -} // namespace octomap -} // namespace easynav -#endif // EASYNAV_OCTOMAP_MAPS_MANAGER_UTILS_HPP_ diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 002b28db..6f4dae23 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -18,7 +18,7 @@ nav_msgs sensor_msgs ament_index_cpp - octomap + octomap_ros octomap_msgs pcl_conversions diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp index 8404d664..a3fd6aad 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp @@ -17,13 +17,10 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include #include #include "easynav_octomap_maps_manager/OctomapMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" #include "easynav_common/RTTFBuffer.hpp" @@ -62,7 +59,7 @@ OctomapMapsManager::OctomapMapsManager() OctomapMapsManager::~OctomapMapsManager() {} -std::expected +void OctomapMapsManager::on_initialize() { auto node = get_node(); @@ -98,7 +95,7 @@ OctomapMapsManager::on_initialize() // if (!result) { // RCLCPP_ERROR(node->get_logger(), // "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - // return std::unexpected("Unable to initialize " + + // throw std::runtime_error("Unable to initialize " + // plugin + " . Error: " + result.error()); // } // @@ -109,7 +106,7 @@ OctomapMapsManager::on_initialize() // } catch (pluginlib::PluginlibException & ex) { // RCLCPP_ERROR(node->get_logger(), // "Unable to load plugin easynav::octomap::OctomapFilter. Error: %s", ex.what()); - // return std::unexpected("Unable to load plugin easynav::octomap::OctomapFilter " + + // throw std::runtime_error("Unable to load plugin easynav::octomap::OctomapFilter " + // octomap_filter + " . Error: " + ex.what()); // } // } @@ -123,13 +120,13 @@ OctomapMapsManager::on_initialize() // const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { -// return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); +// throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); // } // // nav_msgs::msg::OccupancyGrid occ_msg; // if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { // std::cerr << "loadMapFromYaml returned" << ret << std::endl; -// return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); +// throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); // } // // resolution_ = occ_msg.info.resolution; @@ -146,7 +143,7 @@ OctomapMapsManager::on_initialize() // const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { -// return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); +// throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); // } // // if (octomap_ros::io::load_from_file(map_path_, octomap_)) { @@ -173,15 +170,18 @@ OctomapMapsManager::on_initialize() // octomap_pub_->publish(octomap_msg_); // }); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + incoming_pc2_map_sub_ = node->create_subscription( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_pc2_map", rclcpp::QoS(100), - [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { + [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.05)); + tf_info.map_frame, msg->header.frame_id, msg->header.stamp, + rclcpp::Duration::from_seconds(0.05)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "OctomapMapsManager: TF failed: %s", ex.what()); return; @@ -224,8 +224,7 @@ OctomapMapsManager::on_initialize() octomap_->insertPointCloud(cloud, origin, 1000.0, true, false); octomap_->updateInnerOccupancy(); - - octomap_msg_.header.frame_id = "map"; + octomap_msg_.header.frame_id = tf_info.map_frame; octomap_msg_.header.stamp = this->get_node()->now(); octomap_msg_.id = "OcTree"; octomap_msg_.binary = true; @@ -248,8 +247,6 @@ OctomapMapsManager::on_initialize() // ToDo }); - - return {}; } void diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp index c9cce305..0703a15f 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp @@ -39,7 +39,6 @@ *********************************************************************/ -#include #include #include "easynav_common/types/NavState.hpp" @@ -178,7 +177,7 @@ bool InflationFilter::inflate_layer_u8( return true; } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); @@ -197,8 +196,6 @@ InflationFilter::on_initialize() RCLCPP_INFO(node->get_logger(), "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", inflation_radius_, cost_scaling_factor_); - - return {}; } void diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp index 03d82265..ff276efb 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_common/types/NavState.hpp" @@ -40,11 +39,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(::easynav::NavState & nav_state) @@ -58,6 +55,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) const auto & perceptions = nav_state.get("points"); octomap_ = nav_state.get<::octomap::Octomap>("map"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (!octomap_.layer_copy("occupancy", "obstacles")) { RCLCPP_ERROR(parent_node_->get_logger(), "Error copying layers at ObstacleFilter"); @@ -66,7 +64,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(get_map_resolution()) - .fuse(get_tf_prefix() + "map") + .fuse(tf_info.map_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp index 3c732e6d..b25012c6 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp @@ -18,11 +18,9 @@ // along with this program. If not, see . -#include #include #include "octomap/octomap.h" -#include "easynav_common/types/NavState.hpp" #include "easynav_octomap_maps_manager/filters/OctomapFilter.hpp" @@ -36,18 +34,14 @@ OctomapFilter::OctomapFilter() } -std::expected +void OctomapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const std::string & tf_prefix -) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; - - return on_initialize(); + on_initialize(); } std::shared_ptr @@ -62,11 +56,5 @@ OctomapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -OctomapFilter::get_tf_prefix() const -{ - return tf_prefix_; -} - } // namespace octomap } // namespace easynav diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/utils.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/utils.cpp deleted file mode 100644 index 478f6f7e..00000000 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/utils.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include "easynav_octomap_maps_manager/utils.hpp" - -namespace easynav -{ -namespace octomap -{ - - -} // namespace octomap -} // namespace easynav diff --git a/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst new file mode 100644 index 00000000..bc658d4e --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package easynav_simple_maps_manager +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.2 (2025-10-12) +------------------ +* Reorganization initial +* Contributors: Francisco Martín Rico diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt new file mode 100644 index 00000000..18cd1a91 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_routes_maps_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(easynav_costmap_common REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(std_srvs REQUIRED) +find_package(yaets REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(interactive_markers REQUIRED) + + +add_library(${PROJECT_NAME} SHARED + src/easynav_routes_maps_manager/RoutesMapsManager.cpp + src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + easynav_costmap_common::easynav_costmap_common + tf2_ros::tf2_ros + pluginlib::pluginlib + ament_index_cpp::ament_index_cpp + yaets::yaets + yaml-cpp + interactive_markers::interactive_markers + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_srvs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the plugins +pluginlib_export_plugin_description_file(easynav_core easynav_routes_maps_manager_plugins.xml) +pluginlib_export_plugin_description_file(easynav_routes_maps_manager easynav_routes_filters_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + tf2_ros + pluginlib + easynav_costmap_common + nav_msgs + ament_index_cpp + std_srvs + geometry_msgs + visualization_msgs + interactive_markers + yaets + yaml-cpp +) +ament_package() diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md new file mode 100644 index 00000000..30bb391e --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -0,0 +1,155 @@ +# easynav_routes_maps_manager + +## Description + +Maps Manager that maintains a set of navigation routes (in 2D or 3D), exposes them as +visualization markers and interactive markers in RViz, and makes the current +`RoutesMap` available through the NavState so that other managers and filters +can consume it (for example, costmap filters that constrain navigation to a +given corridor). + +Routes are represented as a list of straight-line segments between two poses. +Nothing prevents you from using non-zero Z coordinates in the poses, so +routes can be fully 3D if downstream consumers support it. +They are typically loaded from a YAML file stored in a ROS package and can be +edited at runtime using interactive markers. A default single route segment is +created automatically when no routes file is configured. + +## Authors and Maintainers + +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Francisco Martín Rico + +## Supported ROS 2 Distributions + +| Distribution | Status | +|---|---| +| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | +| kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | + +## Plugin (pluginlib) + +- **Plugin Name:** `easynav_routes_maps_manager/RoutesMapsManager` +- **Type:** `easynav::RoutesMapsManager` +- **Base Class:** `easynav::MapsManagerBase` +- **Library:** `easynav_routes_maps_manager` +- **Description:** Maintains an in-memory set of route segments (RoutesMap), + loads/saves them from/to YAML files, exposes them through ROS markers and + interactive markers, and writes the current `RoutesMap` into the NavState. + +In addition, this package provides the **RoutesCostmapFilter** plugin, which +can be used by the costmap Maps Manager to raise costs outside the defined +routes, effectively constraining navigation to a configurable corridor around +the segments. Other routes filters (for example, for NavMap or Octomap +representations) can be implemented in the same way, each using its own +NavState keys and outputs. + +## Parameters + +### Plugin Parameters (namespace: `//easynav_routes_maps_manager/RoutesMapsManager/...`) + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.package` | `string` | `""` | Package name used to resolve relative route YAML paths via `ament_index`. When empty and `.map_path_file` is also empty, a default in-memory route is created. | +| `.map_path_file` | `string` | `""` | Relative path (inside the package) or absolute path to a YAML file defining the routes (see *Routes YAML Format* below). | +| `.filters` | `string[]` | `[]` | List of routes filter identifiers to be instantiated (see *Routes Filter Plugins*). | +| `..plugin` | `string` | `""` | Type of routes filter plugin (e.g., `easynav_routes_maps_manager/RoutesCostmapFilter`). | + +If both `.package` and `.map_path_file` are empty, the manager +does not attempt to read a file and instead initializes a single default segment +from `(0, 0, 0)` to `(1, 0, 0)`. + +If only one of the parameters is set (package without path or path without +package for relative paths), initialization fails with an error. + +### Routes YAML Format + +Routes are loaded from a YAML file with the following structure: + +```yaml +routes: [route0, route1] + +route0: + start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + +route1: + start: {x: 1.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 2.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +``` + +- `routes`: ordered list of route segment identifiers. +- Each entry under `routeX` defines the start and end poses of a segment. + +When saving routes (via the save service, see below), the manager writes back a +file with the same structure. + +### Routes Filter Plugins + +Each entry in `.filters` defines a sub-namespace `.` +with at least the key `plugin`, plus any filter-specific parameters. Filters +implement the `RoutesFilter` interface and are executed after the `RoutesMap` +has been written to the NavState on each update cycle. + +#### RoutesCostmapFilter + +- **Plugin Name:** `easynav_routes_maps_manager/RoutesCostmapFilter` +- **Type:** `easynav::RoutesCostmapFilter` +- **Description:** + Reads the current `RoutesMap` and a dynamic costmap (`map.dynamic.filtered`) + from the NavState and raises the cost of all cells that do not lie within a + corridor around any route segment. This effectively constrains path planners + to stay close to the defined routes. + +**Parameters (namespace: `//..././...`):** + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| `.min_cost` | `int` | `50` | Minimum cost assigned to cells outside the route corridor if their current cost is lower. | +| `.route_width` | `double` | `0.0` | Corridor half-width around the route segments in meters. When set to `0.0`, a default width equal to half the costmap cell diagonal is used. | + +**NavState usage in RoutesCostmapFilter:** + +| Key | Type | Access | Description | +|---|---|---|---| +| `routes` | `RoutesMap` | **Read** | In-memory set of route segments written by `RoutesMapsManager` on each update. | +| `map.dynamic.filtered` | `Costmap2D` | **Read/Write** | Dynamic costmap that is modified in place: cells outside the corridor are raised to at least `min_cost`. | + +In addition, the filter publishes a debug `nav_msgs/msg/OccupancyGrid` topic +with the filtered costmap for visualization. Other routes filters may use +different NavState keys or map representations; those should document their +own usage in their respective fichas. + +## Interfaces (Topics and Services) + +### Subscriptions and Publications + +| Direction | Topic | Type | Purpose | QoS | +|---|---|---|---|---| +| Publisher | `//routes` | `visualization_msgs/msg/MarkerArray` | Publishes line and arrow markers representing all route segments for RViz visualization. | `depth=10, transient_local, reliable` | +| Publisher | `//routes_imarkers` | `visualization_msgs/msg/InteractiveMarker` | Publishes interactive markers for editing route endpoints (creation, removal, and dragging of segment endpoints). | `depth=1` | +| Publisher | `//routes_map` | `nav_msgs/msg/OccupancyGrid` | (via `RoutesCostmapFilter`) Debug occupancy grid showing the costmap after applying the route-based corridor filter. | `depth=1` | + +### Services + +| Direction | Service | Type | Purpose | +|---|---|---|---| +| Service Server | `//save_routes` | `std_srvs/srv/Trigger` | Serializes the current set of routes to the configured YAML file on disk. | + +## NavState Keys + +| Key | Type | Access | Notes | +|---|---|---|---| +| `routes` | `RoutesMap` | **Write** | In-memory set of route segments loaded from YAML or created interactively. Written by `RoutesMapsManager` on each update. | + +## TF Frames + +Routes are expressed in the navigation `map` frame (or in the frame configured +in other managers that consume the `RoutesMap`). This plugin does not broadcast +TF transforms; it relies on the rest of the navigation stack to provide the +necessary TF tree. + +## License + +GPL-3.0-only diff --git a/maps_managers/easynav_routes_maps_manager/easynav_routes_filters_plugins.xml b/maps_managers/easynav_routes_maps_manager/easynav_routes_filters_plugins.xml new file mode 100644 index 00000000..a000cdd4 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/easynav_routes_filters_plugins.xml @@ -0,0 +1,9 @@ + + + + + Routes-based costmap filter that enforces a minimum cost outside route segments. + + + + diff --git a/maps_managers/easynav_routes_maps_manager/easynav_routes_maps_manager_plugins.xml b/maps_managers/easynav_routes_maps_manager/easynav_routes_maps_manager_plugins.xml new file mode 100644 index 00000000..f286078c --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/easynav_routes_maps_manager_plugins.xml @@ -0,0 +1,11 @@ + + + + + A default "routes" implementation for the Maps Manager. + This maps manager does nothing. It serves as an example, and will be used as a default plugin implementation + if the navigation system configuration does not specify one. + + + + diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp new file mode 100644 index 00000000..192af482 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp @@ -0,0 +1,78 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ + +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace easynav +{ + +class NavState; + +/// @brief Base interface for all routes filters. +/// +/// A RoutesFilter consumes the current navigation state (including the +/// active RoutesMap written by RoutesMapsManager) and is allowed to +/// modify other entries in the NavState (for example, a costmap) in +/// order to enforce corridor-following behaviour, masking, etc. +class RoutesFilter +{ +public: + /// @brief Shared pointer alias for convenience. + using Ptr = std::shared_ptr; + + /// @brief Virtual destructor. + virtual ~RoutesFilter() = default; + + /// @brief Configure the filter instance. + /// + /// This method is called once after construction by the + /// RoutesMapsManager. Implementations should declare/read any + /// required parameters, create publishers or other resources, and + /// store references to the lifecycle node. + /// + /// @param node Shared pointer to the owning lifecycle node. + /// @param plugin_ns Namespace under which this filter is configured + /// (used as prefix for ROS parameters and topics). + /// @param tf_info TF frame information used by the navigation stack. + /// @throws std::runtime_error if initialization fails. + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns) = 0; + + /// @brief Update hook called every navigation cycle. + /// + /// Implementations may read the current "routes" entry from the + /// NavState as well as other map representations and modify them in + /// place. The filter must not assume ownership of data stored in the + /// NavState. + /// + /// @param nav_state Blackboard-like navigation state container. + virtual void update(NavState & nav_state) = 0; +}; + +} // namespace easynav + +#endif // EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp new file mode 100644 index 00000000..37fb17f3 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp @@ -0,0 +1,158 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// \file +/// \brief Declaration of the RoutesMapsManager class and related types. + +#ifndef EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ +#define EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ + +#include "geometry_msgs/msg/pose.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "visualization_msgs/msg/interactive_marker.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" + +#include "interactive_markers/interactive_marker_server.hpp" + +#include "std_srvs/srv/trigger.hpp" + +#include +#include + +#include "pluginlib/class_loader.hpp" + +#include "easynav_core/MapsManagerBase.hpp" + +#include "easynav_routes_maps_manager/RoutesFilter.hpp" + +namespace easynav +{ + +/// @brief Simple directed segment between two poses. +/// +/// Each RouteSegment represents a straight-line connection between two +/// poses in the navigation frame. The segment can be individually +/// edited and identified via its @ref id field. +struct RouteSegment +{ + /// @brief Unique identifier for this segment. + std::string id; + + /// @brief Start pose of the segment. + geometry_msgs::msg::Pose start; + + /// @brief End pose of the segment. + geometry_msgs::msg::Pose end; + + /// @brief Whether this segment is currently in edit mode. + bool edit_mode{false}; +}; + +/// @brief Container type representing a full set of navigation routes. +using RoutesMap = std::vector; + +/** + * @class RoutesMapsManager + * @brief A plugin-based map manager using the RoutesMap data structure. + * + * This manager implements a minimal mapping approach using a set of predefined + * navigation routes (RoutesMap). Routes are defined as straight-line segments + * between two poses and are loaded from a YAML file. The segments are + * visualized using MarkerArray messages. + */ +class RoutesMapsManager : public easynav::MapsManagerBase +{ +public: + /** + * @brief Default constructor. + */ + RoutesMapsManager(); + + /** + * @brief Destructor. + */ + ~RoutesMapsManager(); + + /** + * @brief Initializes the maps manager. + * + * Creates necessary publishers/subscribers and initializes the map instances. + * + * @throws std::runtime_error if initialization fails. + */ + virtual void on_initialize() override; + + /** + * @brief Updates the internal maps using the current navigation state. + * + * Intended to be called periodically. May perform dynamic map updates + * based on new sensor data or internal state. + * + * @param nav_state Current state of the navigation system. + */ + virtual void update(NavState & nav_state) override; + + /// @brief Access to the loaded routes (for testing and tools). + const RoutesMap & get_routes() const {return routes_;} + +private: + /// @brief Load routes from the configured YAML file into @ref routes_. + /// + /// When the configured YAML file is missing, invalid, or the + /// "routes" key is not present, a default map with a single segment + /// from (0, 0, 0) to (1, 0, 0) is created instead. + void load_routes_from_yaml(); + + /// @brief Publish the current routes as visualization markers. + void publish_routes_markers(); + + /// @brief Publish or update interactive markers for editing endpoints. + void publish_interactive_markers(); + + /// @brief Handle feedback from interactive markers and update @ref routes_. + void handle_interactive_feedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); + /// @brief Full path to the YAML file used to load/save routes. + std::string map_path_; + + /// @brief In-memory collection of route segments. + RoutesMap routes_; + + /// @brief Monotonic counter for generating unique route IDs. + int next_route_id_{0}; + + /// @brief Plugin loader for route filters. + std::unique_ptr> routes_filters_loader_; + + /// @brief Loaded route filters. + std::vector> routes_filters_; + + /// @brief Publisher for visualizing routes as markers. + rclcpp::Publisher::SharedPtr routes_pub_; + + /// @brief Interactive marker server for editing endpoints. + std::shared_ptr imarker_server_; + + /// @brief Service for saving current routes back to disk. + rclcpp::Service::SharedPtr save_routes_srv_; +}; + +} // namespace easynav + +#endif // EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp new file mode 100644 index 00000000..c910a916 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp @@ -0,0 +1,98 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ + +#include + +#include "easynav_routes_maps_manager/RoutesFilter.hpp" + +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace easynav +{ + +/// @brief Costmap filter that raises costs outside navigation routes. +/// +/// This filter reads the current RoutesMap and a dynamic costmap +/// ("map.dynamic.filtered") from the NavState. All cells that do not +/// lie within a configurable corridor around any route segment have +/// their cost raised to at least @c min_cost_. A debug occupancy grid +/// representing the filtered costmap is also published. +class RoutesCostmapFilter : public RoutesFilter +{ +public: + /// @brief Default constructor. + RoutesCostmapFilter(); + + /// @brief Configure the costmap filter. + /// + /// Declares and reads plugin-specific parameters (such as + /// @c min_cost and @c route_width), and creates the debug + /// OccupancyGrid publisher. + /// + /// @param node Owning lifecycle node provided by the + /// RoutesMapsManager. + /// @param plugin_ns Namespace used to resolve this filter's + /// parameters and topics. + /// @param tf_info TF frame information used by the navigation stack. + /// @throws std::runtime_error if initialization fails. + void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns) override; + + /// @brief Apply the routes-based filtering to the costmap. + /// + /// If both "routes" and "map.dynamic.filtered" entries are present + /// in the NavState, the costmap is modified in place and the debug + /// occupancy grid is published. If either entry is missing, the + /// function returns without making changes. + /// + /// @param nav_state Current navigation state containing the routes + /// and costmap to be filtered. + void update(NavState & nav_state) override; + +private: + /// @brief Weak reference to the owning lifecycle node. + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + /// @brief Plugin namespace used for parameters and topics. + std::string plugin_ns_; + + /// @brief Minimum cost enforced outside the route corridor. + int min_cost_{50}; + + /// @brief Corridor half-width around route segments in meters. + /// + /// When set to @c 0.0, a default corresponding to half of the + /// costmap cell diagonal is used. + double route_width_{0.0}; + + /// @brief Publisher for the debug occupancy grid representing the + /// filtered costmap. + rclcpp::Publisher::SharedPtr routes_occ_pub_; + + /// @brief Reusable message used to publish the debug grid. + nav_msgs::msg::OccupancyGrid routes_grid_msg_; +}; + +} // namespace easynav + +#endif // EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml new file mode 100644 index 00000000..2bcb255a --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -0,0 +1,35 @@ + + + + easynav_routes_maps_manager + 0.0.2 + Easy Navigation: Routes MapsManager package. + Francisco Martín Rico + GPL-3.0-only + + ament_cmake + + + easynav_common + easynav_core + easynav_costmap_common + tf2_ros + pluginlib + nav_msgs + ament_index_cpp + std_srvs + geometry_msgs + visualization_msgs + interactive_markers + yaets + yaml-cpp + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp new file mode 100644 index 00000000..2601842e --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp @@ -0,0 +1,797 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include + +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace easynav +{ + +RoutesMapsManager::RoutesMapsManager() +{ + NavState::register_printer( + [](const RoutesMap & routes) { + std::ostringstream out; + out << "RoutesMap with " << routes.size() << " segments"; + for (std::size_t i = 0; i < routes.size(); ++i) { + const auto & s = routes[i]; + out << "\n [" << i << "] from (" << s.start.position.x << ", " + << s.start.position.y << ") to (" << s.end.position.x << ", " + << s.end.position.y << ")"; + } + return out.str(); + }); + + routes_filters_loader_ = std::make_unique>( + "easynav_routes_maps_manager", "easynav::RoutesFilter"); +} + +RoutesMapsManager::~RoutesMapsManager() = default; + +void RoutesMapsManager::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + std::string package_name, map_path_file; + if (!node->has_parameter(plugin_name + ".package")) { + node->declare_parameter(plugin_name + ".package", package_name); + } + if (!node->has_parameter(plugin_name + ".map_path_file")) { + node->declare_parameter(plugin_name + ".map_path_file", map_path_file); + } + + node->get_parameter(plugin_name + ".package", package_name); + node->get_parameter(plugin_name + ".map_path_file", map_path_file); + + // Load route filters plugins configuration + std::vector routes_filters_names; + if (!node->has_parameter(plugin_name + ".filters")) { + node->declare_parameter(plugin_name + ".filters", routes_filters_names); + } + node->get_parameter(plugin_name + ".filters", routes_filters_names); + + map_path_.clear(); + if (package_name.empty() && map_path_file.empty()) { + // Accepted: we will use a default in-memory route in load_routes_from_yaml(). + RCLCPP_INFO( + node->get_logger(), + "[%s] No package or map_path_file specified, using default in-memory route", + plugin_name.c_str()); + } else if (!map_path_file.empty() && map_path_file[0] == '/') { + // Absolute path: ignore package_name. + map_path_ = map_path_file; + } else if (!package_name.empty() && !map_path_file.empty()) { + const auto pkgpath = ament_index_cpp::get_package_share_directory(package_name); + map_path_ = pkgpath + "/" + map_path_file; + } else { + throw std::runtime_error( + "Parameters '" + plugin_name + ".package' and '" + plugin_name + + ".map_path_file' are not correctly set"); + } + + routes_pub_ = node->create_publisher( + node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes", + rclcpp::QoS(10).transient_local().reliable()); + + imarker_server_ = std::make_shared( + plugin_name + std::string("_imarkers"), node, false); + + save_routes_srv_ = node->create_service( + node->get_fully_qualified_name() + std::string("/") + plugin_name + "/save_routes", + [this](const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr response) { + try { + // Persist current routes_ back to YAML file using the + // structure: + // routes: [route1, route2] + // route1: { start: ..., end: ... } + YAML::Emitter out; + out << YAML::BeginMap; + + // Collect route names from ids (or generate generic ones). + std::vector names; + names.reserve(routes_.size()); + for (std::size_t i = 0; i < routes_.size(); ++i) { + const auto & seg = routes_[i]; + if (!seg.id.empty()) { + names.push_back(seg.id); + } else { + names.push_back("route" + std::to_string(i)); + } + } + + out << YAML::Key << "routes" << YAML::Value << YAML::Flow << YAML::BeginSeq; + for (const auto & n : names) { + out << n; + } + out << YAML::EndSeq; + + // Now define each route as a separate key in the map. + for (std::size_t i = 0; i < routes_.size(); ++i) { + const auto & seg = routes_[i]; + const auto & name = names[i]; + + out << YAML::Key << name << YAML::Value << YAML::BeginMap; + + out << YAML::Key << "start" << YAML::Value << YAML::BeginMap; + out << YAML::Key << "x" << YAML::Value << seg.start.position.x; + out << YAML::Key << "y" << YAML::Value << seg.start.position.y; + out << YAML::Key << "z" << YAML::Value << seg.start.position.z; + out << YAML::Key << "qx" << YAML::Value << seg.start.orientation.x; + out << YAML::Key << "qy" << YAML::Value << seg.start.orientation.y; + out << YAML::Key << "qz" << YAML::Value << seg.start.orientation.z; + out << YAML::Key << "qw" << YAML::Value << seg.start.orientation.w; + out << YAML::EndMap; + + out << YAML::Key << "end" << YAML::Value << YAML::BeginMap; + out << YAML::Key << "x" << YAML::Value << seg.end.position.x; + out << YAML::Key << "y" << YAML::Value << seg.end.position.y; + out << YAML::Key << "z" << YAML::Value << seg.end.position.z; + out << YAML::Key << "qx" << YAML::Value << seg.end.orientation.x; + out << YAML::Key << "qy" << YAML::Value << seg.end.orientation.y; + out << YAML::Key << "qz" << YAML::Value << seg.end.orientation.z; + out << YAML::Key << "qw" << YAML::Value << seg.end.orientation.w; + out << YAML::EndMap; + + out << YAML::EndMap; + } + + out << YAML::EndMap; + + std::ofstream file(map_path_); + file << out.c_str(); + file.close(); + + response->success = true; + response->message = "Routes saved"; + } catch (const std::exception & e) { + response->success = false; + response->message = e.what(); + } + }); + + try { + load_routes_from_yaml(); + publish_routes_markers(); + publish_interactive_markers(); + } catch (const std::exception & e) { + throw std::runtime_error(std::string{"Failed to load routes: "} + e.what()); + } + + // Instantiate and initialize configured route filters + for (const auto & filter_name : routes_filters_names) { + std::string plugin; + if (!node->has_parameter(plugin_name + "." + filter_name + ".plugin")) { + node->declare_parameter(plugin_name + "." + filter_name + ".plugin", plugin); + } + node->get_parameter(plugin_name + "." + filter_name + ".plugin", plugin); + + if (plugin.empty()) { + RCLCPP_WARN(node->get_logger(), + "RoutesMapsManager: plugin parameter for filter '%s' is empty", filter_name.c_str()); + continue; + } + + try { + RCLCPP_INFO(node->get_logger(), + "Loading RoutesFilter %s [%s]", filter_name.c_str(), plugin.c_str()); + + std::shared_ptr instance = + routes_filters_loader_->createSharedInstance(plugin); + + try { + instance->initialize(node, plugin_name + "." + filter_name); + } catch (const std::exception & e) { + RCLCPP_ERROR(node->get_logger(), + "Unable to initialize RoutesFilter %s [%s]. Error: %s", + filter_name.c_str(), plugin.c_str(), e.what()); + throw std::runtime_error("Unable to initialize RoutesFilter " + plugin + + " . Error: " + e.what()); + } + + routes_filters_.push_back(instance); + + RCLCPP_INFO(node->get_logger(), + "Loaded RoutesFilter %s [%s]", filter_name.c_str(), plugin.c_str()); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_ERROR(node->get_logger(), + "Unable to load plugin easynav::RoutesFilter. Error: %s", ex.what()); + throw std::runtime_error("Unable to load plugin easynav::RoutesFilter " + + filter_name + " . Error: " + ex.what()); + } + } +} + +void RoutesMapsManager::update(NavState & nav_state) +{ + // Expose routes map through NavState for other modules. + nav_state.set("routes", routes_); + + // Let route filters apply their effects based on current routes and nav state. + for (const auto & filter : routes_filters_) { + filter->update(nav_state); + } +} + +void RoutesMapsManager::load_routes_from_yaml() +{ + routes_.clear(); + + if (map_path_.empty()) { + // No map path configured: initialize a default single route segment. + RouteSegment segment; + segment.id = "route0"; + segment.start.position.x = 0.0; + segment.start.position.y = 0.0; + segment.start.position.z = 0.0; + segment.start.orientation.x = 0.0; + segment.start.orientation.y = 0.0; + segment.start.orientation.z = 0.0; + segment.start.orientation.w = 1.0; + + segment.end.position.x = 1.0; + segment.end.position.y = 0.0; + segment.end.position.z = 0.0; + segment.end.orientation.x = 0.0; + segment.end.orientation.y = 0.0; + segment.end.orientation.z = 0.0; + segment.end.orientation.w = 1.0; + + routes_.push_back(segment); + next_route_id_ = 1; + return; + } + + YAML::Node root; + try { + root = YAML::LoadFile(map_path_); + } catch (const std::exception &) { + // File missing or invalid: fall back to a default single route. + RouteSegment segment; + segment.id = "route0"; + segment.start.position.x = 0.0; + segment.start.position.y = 0.0; + segment.start.position.z = 0.0; + segment.start.orientation.x = 0.0; + segment.start.orientation.y = 0.0; + segment.start.orientation.z = 0.0; + segment.start.orientation.w = 1.0; + + segment.end.position.x = 1.0; + segment.end.position.y = 0.0; + segment.end.position.z = 0.0; + segment.end.orientation.x = 0.0; + segment.end.orientation.y = 0.0; + segment.end.orientation.z = 0.0; + segment.end.orientation.w = 1.0; + + routes_.push_back(segment); + next_route_id_ = 1; + return; + } + + if (!root["routes"]) { + // No explicit routes list: use a default single route. + RouteSegment segment; + segment.id = "route0"; + segment.start.position.x = 0.0; + segment.start.position.y = 0.0; + segment.start.position.z = 0.0; + segment.start.orientation.x = 0.0; + segment.start.orientation.y = 0.0; + segment.start.orientation.z = 0.0; + segment.start.orientation.w = 1.0; + + segment.end.position.x = 1.0; + segment.end.position.y = 0.0; + segment.end.position.z = 0.0; + segment.end.orientation.x = 0.0; + segment.end.orientation.y = 0.0; + segment.end.orientation.z = 0.0; + segment.end.orientation.w = 1.0; + + routes_.push_back(segment); + next_route_id_ = 1; + return; + } + + // routes: [route1, route2, ...] + const auto & names_node = root["routes"]; + for (std::size_t i = 0; i < names_node.size(); ++i) { + const auto name = names_node[i].as(); + + if (!root[name]) { + continue; + } + + const auto & route_node = root[name]; + if (!route_node["start"] || !route_node["end"]) { + continue; + } + + RouteSegment segment; + segment.id = name; + + const auto & start = route_node["start"]; + const auto & end = route_node["end"]; + + segment.start.position.x = start["x"].as(); + segment.start.position.y = start["y"].as(); + segment.start.position.z = start["z"].as(0.0); + + segment.start.orientation.x = start["qx"].as(0.0); + segment.start.orientation.y = start["qy"].as(0.0); + segment.start.orientation.z = start["qz"].as(0.0); + segment.start.orientation.w = start["qw"].as(1.0); + + segment.end.position.x = end["x"].as(); + segment.end.position.y = end["y"].as(); + segment.end.position.z = end["z"].as(0.0); + + segment.end.orientation.x = end["qx"].as(0.0); + segment.end.orientation.y = end["qy"].as(0.0); + segment.end.orientation.z = end["qz"].as(0.0); + segment.end.orientation.w = end["qw"].as(1.0); + + routes_.push_back(segment); + } + + // Initialize next_route_id_ so that newly created routes get + // unique IDs that don't clash with existing ones. + next_route_id_ = 0; + for (const auto & seg : routes_) { + if (seg.id.rfind("route", 0) == 0 && seg.id.size() > 5) { + try { + const int n = std::stoi(seg.id.substr(5)); + if (n >= next_route_id_) { + next_route_id_ = n + 1; + } + } catch (...) { + // Non-numeric suffixes are ignored. + } + } + } +} + +void RoutesMapsManager::publish_routes_markers() +{ + if (!routes_pub_) { + return; + } + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + visualization_msgs::msg::MarkerArray array; + + // First, delete all previous markers in our namespaces so that + // removed segments do not leave orphaned markers behind. + { + visualization_msgs::msg::Marker m; + m.header.frame_id = tf_info.map_frame; + m.action = visualization_msgs::msg::Marker::DELETEALL; + + m.ns = "routes_line"; + array.markers.push_back(m); + + m.ns = "routes_arrow"; + array.markers.push_back(m); + } + + int id = 0; + for (const auto & seg : routes_) { + // Line between start and end + visualization_msgs::msg::Marker line; + line.header.frame_id = tf_info.map_frame; + line.ns = "routes_line"; + line.id = id++; + line.type = visualization_msgs::msg::Marker::LINE_LIST; + line.action = visualization_msgs::msg::Marker::ADD; + + line.scale.x = 0.05; // line width + + line.color.r = 0.0f; + line.color.g = 1.0f; + line.color.b = 0.0f; + line.color.a = 1.0f; + + line.points.resize(2); + line.points[0].x = seg.start.position.x; + line.points[0].y = seg.start.position.y; + line.points[0].z = seg.start.position.z; + + line.points[1].x = seg.end.position.x; + line.points[1].y = seg.end.position.y; + line.points[1].z = seg.end.position.z; + + array.markers.push_back(line); + + // Arrow for start orientation (same style as end) + visualization_msgs::msg::Marker start_arrow; + start_arrow.header.frame_id = tf_info.map_frame; + start_arrow.ns = "routes_arrow"; + start_arrow.id = id++; + start_arrow.type = visualization_msgs::msg::Marker::ARROW; + start_arrow.action = visualization_msgs::msg::Marker::ADD; + start_arrow.pose = seg.start; + start_arrow.scale.x = 0.25; // shaft length + start_arrow.scale.y = 0.05; // shaft diameter + start_arrow.scale.z = 0.1; // head diameter + start_arrow.color.r = 1.0f; + start_arrow.color.g = 1.0f; + start_arrow.color.b = 0.0f; + start_arrow.color.a = 0.9f; + array.markers.push_back(start_arrow); + + // Arrow for end orientation (same style) + visualization_msgs::msg::Marker end_arrow; + end_arrow.header.frame_id = tf_info.map_frame; + end_arrow.ns = "routes_arrow"; + end_arrow.id = id++; + end_arrow.type = visualization_msgs::msg::Marker::ARROW; + end_arrow.action = visualization_msgs::msg::Marker::ADD; + end_arrow.pose = seg.end; + end_arrow.scale.x = 0.25; + end_arrow.scale.y = 0.05; + end_arrow.scale.z = 0.1; + end_arrow.color.r = 1.0f; + end_arrow.color.g = 1.0f; + end_arrow.color.b = 0.0f; + end_arrow.color.a = 0.9f; + array.markers.push_back(end_arrow); + } + + routes_pub_->publish(array); +} + +void RoutesMapsManager::publish_interactive_markers() +{ + if (!imarker_server_) { + return; + } + imarker_server_->clear(); + + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + for (const auto & seg : routes_) { + // Per-segment toggle cube (red in normal mode, green in edit mode). + visualization_msgs::msg::InteractiveMarker mode_marker; + mode_marker.header.frame_id = tf_info.map_frame; + mode_marker.name = seg.id + "_mode"; + mode_marker.scale = 1.0; + + // Mid-point between start and end (no vertical offset) + mode_marker.pose.position.x = 0.5 * (seg.start.position.x + seg.end.position.x); + mode_marker.pose.position.y = 0.5 * (seg.start.position.y + seg.end.position.y); + mode_marker.pose.position.z = 0.5 * (seg.start.position.z + seg.end.position.z); + + visualization_msgs::msg::InteractiveMarkerControl mode_ctrl; + mode_ctrl.name = "toggle_edit"; + mode_ctrl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::BUTTON; + mode_ctrl.always_visible = true; + + visualization_msgs::msg::Marker cube; + cube.type = visualization_msgs::msg::Marker::CUBE; + cube.scale.x = 0.15; // even smaller cube + cube.scale.y = 0.15; + cube.scale.z = 0.15; + if (!seg.edit_mode) { + // Red in normal mode + cube.color.r = 1.0f; + cube.color.g = 0.0f; + cube.color.b = 0.0f; + } else { + // Green in edit mode + cube.color.r = 0.0f; + cube.color.g = 1.0f; + cube.color.b = 0.0f; + } + cube.color.a = 0.9f; + + // Text label for the toggle control + visualization_msgs::msg::Marker toggle_text; + toggle_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + toggle_text.text = "toggle edit"; + toggle_text.scale.z = 0.1; // font height + toggle_text.color.r = 1.0f; + toggle_text.color.g = 1.0f; + toggle_text.color.b = 1.0f; + toggle_text.color.a = 1.0f; + toggle_text.pose.position.z = 0.3; // slightly above the cube + + mode_ctrl.markers.push_back(cube); + mode_ctrl.markers.push_back(toggle_text); + + mode_marker.controls.push_back(mode_ctrl); + + imarker_server_->insert( + mode_marker, + std::bind( + &RoutesMapsManager::handle_interactive_feedback, + this, + std::placeholders::_1)); + + if (!seg.edit_mode) { + // In normal mode we only show the cube and skip endpoint controls. + continue; + } + + visualization_msgs::msg::InteractiveMarker start_marker; + start_marker.header.frame_id = tf_info.map_frame; + start_marker.name = seg.id + "_start"; + start_marker.description = "Route " + seg.id + " start"; + start_marker.pose = seg.start; + start_marker.scale = 1.0; + + visualization_msgs::msg::InteractiveMarker end_marker; + end_marker.header.frame_id = tf_info.map_frame; + end_marker.name = seg.id + "_end"; + end_marker.description = "Route " + seg.id + " end"; + end_marker.pose = seg.end; + end_marker.scale = 1.0; + + auto add_controls = [](visualization_msgs::msg::InteractiveMarker & marker) { + visualization_msgs::msg::InteractiveMarkerControl control; + + // Move along X + control.orientation.w = 1.0; + control.orientation.x = 1.0; + control.orientation.y = 0.0; + control.orientation.z = 0.0; + control.name = "move_x"; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + marker.controls.push_back(control); + + // Move along Y + control.orientation.x = 0.0; + control.orientation.y = 1.0; + control.name = "move_y"; + marker.controls.push_back(control); + + // Move along Z + control.orientation.y = 0.0; + control.orientation.z = 1.0; + control.name = "move_z"; + marker.controls.push_back(control); + + // Rotate around Z (yaw), orientation as in interactive_markers examples + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; + control.orientation.w = 1.0; + control.orientation.x = 0.0; + control.orientation.y = 1.0; + control.orientation.z = 0.0; + control.name = "rotate_z"; + marker.controls.push_back(control); + + // Button control to add a new segment starting from this endpoint + visualization_msgs::msg::InteractiveMarkerControl add_ctrl; + add_ctrl.name = "add_segment"; + add_ctrl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::BUTTON; + add_ctrl.always_visible = true; + + visualization_msgs::msg::Marker add_marker; + add_marker.type = visualization_msgs::msg::Marker::SPHERE; + // Visual sphere for the add control + add_marker.scale.x = 0.2; + add_marker.scale.y = 0.2; + add_marker.scale.z = 0.2; + add_marker.color.r = 1.0f; + add_marker.color.g = 0.5f; + add_marker.color.b = 0.0f; + add_marker.color.a = 0.9f; + + // Text label for the add control + visualization_msgs::msg::Marker add_text; + add_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + add_text.text = "add"; + add_text.scale.z = 0.1; // font height (half) + add_text.color.r = 1.0f; + add_text.color.g = 1.0f; + add_text.color.b = 1.0f; + add_text.color.a = 1.0f; + add_text.pose.position.z = 0.4; // slightly above the sphere + + add_ctrl.markers.push_back(add_marker); + add_ctrl.markers.push_back(add_text); + + marker.controls.push_back(add_ctrl); + + // Button control to remove the segment this endpoint belongs to + visualization_msgs::msg::InteractiveMarkerControl remove_ctrl; + remove_ctrl.name = "remove_segment"; + remove_ctrl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::BUTTON; + remove_ctrl.always_visible = true; + + visualization_msgs::msg::Marker remove_marker; + remove_marker.type = visualization_msgs::msg::Marker::SPHERE; + // Place the red sphere 1 m above the endpoint so that it + // does not overlap with the orange "add" sphere. + remove_marker.pose.position.z = 1.0; + remove_marker.scale.x = 0.15; + remove_marker.scale.y = 0.15; + remove_marker.scale.z = 0.15; + remove_marker.color.r = 1.0f; + remove_marker.color.g = 0.0f; + remove_marker.color.b = 0.0f; + remove_marker.color.a = 0.9f; + + // Text label for the remove control + visualization_msgs::msg::Marker remove_text; + remove_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + remove_text.text = "remove"; + remove_text.scale.z = 0.1; // font height (half) + remove_text.color.r = 1.0f; + remove_text.color.g = 1.0f; + remove_text.color.b = 1.0f; + remove_text.color.a = 1.0f; + remove_text.pose.position.z = 1.3; // slightly above the red sphere + + remove_ctrl.markers.push_back(remove_marker); + remove_ctrl.markers.push_back(remove_text); + + marker.controls.push_back(remove_ctrl); + }; + + add_controls(start_marker); + add_controls(end_marker); + + imarker_server_->insert( + start_marker, + std::bind( + &RoutesMapsManager::handle_interactive_feedback, + this, + std::placeholders::_1)); + imarker_server_->insert( + end_marker, + std::bind( + &RoutesMapsManager::handle_interactive_feedback, + this, + std::placeholders::_1)); + } + + imarker_server_->applyChanges(); +} + +void RoutesMapsManager::handle_interactive_feedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) +{ + if (!feedback) { + return; + } + + const auto & name = feedback->marker_name; + + // Toggle per-segment edit mode when clicking the central cube. + if (feedback->control_name == "toggle_edit" && + (feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::BUTTON_CLICK || + feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP)) + { + // name is _mode + const auto underscore_pos = name.rfind("_"); + if (underscore_pos != std::string::npos) { + const auto base_id = name.substr(0, underscore_pos); + for (auto & seg : routes_) { + if (seg.id == base_id) { + seg.edit_mode = !seg.edit_mode; + break; + } + } + publish_interactive_markers(); + } + return; + } + + // Creation of a new segment from this endpoint + if (feedback->control_name == "add_segment" && + (feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::BUTTON_CLICK || + feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP)) + { + // Compute forward direction from marker orientation (assume x-forward). + const auto & p = feedback->pose.position; + const auto & q = feedback->pose.orientation; + + const double qx = q.x; + const double qy = q.y; + const double qz = q.z; + const double qw = q.w; + + // Forward vector in world coordinates: q * (1,0,0) * q^{-1} + const double fx = 2.0 * (qx * qx + qw * qw) - 1.0; + const double fy = 2.0 * (qx * qy + qw * qz); + const double fz = 2.0 * (qx * qz - qw * qy); + + const double length = 2.0; // meters + + RouteSegment new_seg; + // New unique segment id based on a monotonic counter + new_seg.id = "route" + std::to_string(next_route_id_++); + + new_seg.start = feedback->pose; + new_seg.end = feedback->pose; + new_seg.end.position.x = p.x + fx * length; + new_seg.end.position.y = p.y + fy * length; + new_seg.end.position.z = p.z + fz * length; + + routes_.push_back(new_seg); + + publish_routes_markers(); + publish_interactive_markers(); + return; + } + + // Removal of the segment this endpoint belongs to + if (feedback->control_name == "remove_segment" && + (feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::BUTTON_CLICK || + feedback->event_type == + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP)) + { + // marker_name is either _start or _end + const auto underscore_pos = name.rfind("_"); + if (underscore_pos != std::string::npos) { + const auto base_id = name.substr(0, underscore_pos); + for (auto it = routes_.begin(); it != routes_.end(); ++it) { + if (it->id == base_id) { + routes_.erase(it); + break; + } + } + publish_routes_markers(); + publish_interactive_markers(); + } + return; + } + + for (auto & seg : routes_) { + if (name == seg.id + "_start") { + seg.start = feedback->pose; + publish_routes_markers(); + publish_interactive_markers(); + return; + } else if (name == seg.id + "_end") { + seg.end = feedback->pose; + publish_routes_markers(); + publish_interactive_markers(); + return; + } + } +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::RoutesMapsManager, easynav::MapsManagerBase) diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp new file mode 100644 index 00000000..83908e19 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp @@ -0,0 +1,192 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + + +#include "easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp" + +#include +#include +#include + +#include "easynav_common/types/NavState.hpp" + +#include "easynav_costmap_common/costmap_2d.hpp" + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +namespace easynav +{ + +RoutesCostmapFilter::RoutesCostmapFilter() = default; + +void +RoutesCostmapFilter::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns) +{ + node_ = node; + plugin_ns_ = plugin_ns; + // Parameter for minimum cost to apply outside routes + if (!node->has_parameter(plugin_ns_ + ".min_cost")) { + node->declare_parameter(plugin_ns_ + ".min_cost", min_cost_); + } + node->get_parameter(plugin_ns_ + ".min_cost", min_cost_); + + // Width of the route corridor around the segment (meters). + // If set to 0.0 (default), a width of one cell is used. + if (!node->has_parameter(plugin_ns_ + ".route_width")) { + node->declare_parameter(plugin_ns_ + ".route_width", route_width_); + } + node->get_parameter(plugin_ns_ + ".route_width", route_width_); + + // Publisher for the routes-influenced occupancy grid (for debugging/visualization) + // Topic: ///routes_map + // plugin_ns_ is typically ".", so we extract the + // base plugin_name (e.g. "routes" from "routes.routes_costmap"). + const auto dot_pos = plugin_ns_.find('.'); + const std::string plugin_name = + (dot_pos == std::string::npos) ? plugin_ns_ : plugin_ns_.substr(0, dot_pos); + + routes_occ_pub_ = node->create_publisher( + node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes_map", + rclcpp::QoS(1).reliable()); +} + +void +RoutesCostmapFilter::update(NavState & nav_state) +{ + if (!nav_state.has("routes")) { + return; + } + + if (!nav_state.has("map.dynamic.filtered")) { + return; + } + + const auto & routes = nav_state.get("routes"); + + auto costmap_ptr = nav_state.get_ptr("map.dynamic.filtered"); + if (!costmap_ptr) { + return; + } + + Costmap2D & costmap = *costmap_ptr; + + const double resolution = costmap.getResolution(); + const double origin_x = costmap.getOriginX(); + const double origin_y = costmap.getOriginY(); + + const unsigned int size_x = costmap.getSizeInCellsX(); + const unsigned int size_y = costmap.getSizeInCellsY(); + + // Precompute a list of segments in world coordinates + struct Segment2D + { + double x0, y0, x1, y1; + }; + + std::vector segments; + segments.reserve(routes.size()); + for (const auto & seg : routes) { + segments.push_back({ + seg.start.position.x, + seg.start.position.y, + seg.end.position.x, + seg.end.position.y}); + } + + auto cellHasRoute = [&segments, resolution, origin_x, origin_y, this]( + unsigned int mx, unsigned int my) -> bool + { + // Cell center in world coordinates + const double cx = origin_x + (mx + 0.5) * resolution; + const double cy = origin_y + (my + 0.5) * resolution; + + for (const auto & s : segments) { + // Distance from (cx,cy) to segment (s.x0,s.y0)-(s.x1,s.y1) + const double vx = s.x1 - s.x0; + const double vy = s.y1 - s.y0; + const double wx = cx - s.x0; + const double wy = cy - s.y0; + + const double c1 = vx * wx + vy * wy; + const double c2 = vx * vx + vy * vy; + + // Allow projection also before the start point (t >= 0) + if (c2 <= 0.0) { + continue; + } + const double t = c1 / c2; + if (t < 0.0 || t > 1.0) { + continue; + } + + const double proj_x = s.x0 + t * vx; + const double proj_y = s.y0 + t * vy; + + const double dx = cx - proj_x; + const double dy = cy - proj_y; + const double dist2 = dx * dx + dy * dy; + + // Consider the cell as on the route if the distance is less than + // the configured route width (or, if zero, half the cell diagonal). + double max_dist = route_width_; + if (max_dist <= 0.0) { + max_dist = std::sqrt(2.0) * resolution * 0.5; + } + + // Make the boundary slightly inclusive with a small epsilon to + // avoid dropping edge cells due to numerical issues. + constexpr double epsilon = 1e-6; + if (dist2 <= (max_dist * max_dist) + epsilon) { + return true; + } + } + + return false; + }; + + for (unsigned int y = 0; y < size_y; ++y) { + for (unsigned int x = 0; x < size_x; ++x) { + if (cellHasRoute(x, y)) { + // Do not modify cost on route + continue; + } + + unsigned char old_cost = costmap.getCost(x, y); + if (old_cost < static_cast(min_cost_)) { + costmap.setCost(x, y, static_cast(min_cost_)); + } + } + } + + costmap.toOccupancyGridMsg(routes_grid_msg_); + routes_grid_msg_.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame; + if (auto node_locked = node_.lock()) { + routes_grid_msg_.header.stamp = node_locked->now(); + } + routes_occ_pub_->publish(routes_grid_msg_); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::RoutesCostmapFilter, easynav::RoutesFilter) diff --git a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt new file mode 100644 index 00000000..3638b4d4 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -0,0 +1,19 @@ +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(routes_mapsmanager_tests + routes_mapsmanager_tests.cpp) +target_link_libraries(routes_mapsmanager_tests + ${PROJECT_NAME} + easynav_common::easynav_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle) + +ament_add_gtest(routes_costmap_filter_tests + routes_costmap_filter_tests.cpp) +target_link_libraries(routes_costmap_filter_tests + ${PROJECT_NAME} + easynav_common::easynav_common + easynav_costmap_common::easynav_costmap_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle) + diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp new file mode 100644 index 00000000..40d8c72b --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp @@ -0,0 +1,165 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include + +#include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_costmap_common/costmap_2d.hpp" + +#include "easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp" + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" // for RoutesMap and RouteSegment typedefs + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using easynav::RoutesCostmapFilter; +using easynav::RoutesMap; +using easynav::RouteSegment; +using easynav::NavState; +using easynav::Costmap2D; + +class RoutesCostmapFilterTest : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) +{ + auto node = std::make_shared("routes_costmap_filter_missing"); + + RoutesCostmapFilter filter; + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); + + NavState nav_state; + // No 'routes' and no 'map.dynamic.filtered' keys -> update should not crash + EXPECT_NO_THROW(filter.update(nav_state)); +} + +TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) +{ + auto node = std::make_shared("routes_costmap_filter_basic"); + + RoutesCostmapFilter filter; + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); + + // Simple costmap 10x1, resolution 1.0, origin at (0,0) + Costmap2D map(10, 1, 1.0, 0.0, 0.0); + // Initialize all costs to 0 + for (unsigned int x = 0; x < 10; ++x) { + map.setCost(x, 0, 0); + } + + // Single route from (2,0) to (7,0) + RoutesMap routes; + RouteSegment seg; + seg.id = "route0"; + seg.start.position.x = 2.0; + seg.start.position.y = 0.0; + seg.start.orientation.w = 1.0; + seg.end.position.x = 7.0; + seg.end.position.y = 0.0; + seg.end.orientation.w = 1.0; + routes.push_back(seg); + + NavState nav_state; + nav_state.set("routes", routes); + nav_state.set("map.dynamic.filtered", map); + + filter.update(nav_state); + + auto map_after = nav_state.get("map.dynamic.filtered"); + + // Cells clearly along the interior of the route should remain 0, + // others should be >= 50 (default min_cost). The last endpoint + // cell (x=7) may fall outside the corridor depending on the + // distance test, so we only assert [2,6] as "on route" here. + for (unsigned int x = 0; x < 10; ++x) { + if (x >= 2 && x <= 6) { + EXPECT_EQ(map_after.getCost(x, 0), 0) << "Cell on route changed at x=" << x; + } else { + EXPECT_GE(map_after.getCost(x, 0), 50) << "Cell outside route not raised at x=" << x; + } + } +} + +TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) +{ + auto node = std::make_shared("routes_costmap_filter_outside"); + + RoutesCostmapFilter filter; + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); + + // Costmap 5x1 from x=0..5 + Costmap2D map(5, 1, 1.0, 0.0, 0.0); + for (unsigned int x = 0; x < 5; ++x) { + map.setCost(x, 0, 0); + } + + // Route completely outside map, e.g., from (10,0) to (11,0) + RoutesMap routes; + RouteSegment seg; + seg.id = "route_outside"; + seg.start.position.x = 10.0; + seg.start.position.y = 0.0; + seg.start.orientation.w = 1.0; + seg.end.position.x = 11.0; + seg.end.position.y = 0.0; + seg.end.orientation.w = 1.0; + routes.push_back(seg); + + NavState nav_state; + nav_state.set("routes", routes); + nav_state.set("map.dynamic.filtered", map); + + filter.update(nav_state); + + auto map_after = nav_state.get("map.dynamic.filtered"); + + // No cell should be considered "on route"; all should be raised to >= 50 + for (unsigned int x = 0; x < 5; ++x) { + EXPECT_GE(map_after.getCost(x, 0), 50) << "Cell not raised at x=" << x; + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp new file mode 100644 index 00000000..665a25a7 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -0,0 +1,224 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + + +#include +#include + +#include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using easynav::RoutesMapsManager; +using easynav::RoutesMap; +using easynav::RouteSegment; + +class RoutesMapsManagerTest : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +// Helper to create a temporary YAML file with given contents +static std::string create_temp_yaml(const std::string & contents) +{ + char filename[] = "/tmp/routes_test_XXXXXX.yaml"; + int fd = mkstemps(filename, 5); // keep .yaml suffix + if (fd == -1) { + throw std::runtime_error("Unable to create temp file"); + } + close(fd); + + std::ofstream out(filename); + out << contents; + out.close(); + return std::string(filename); +} + +TEST_F(RoutesMapsManagerTest, LoadsRoutesFromValidYaml) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node"); + + const std::string yaml = + "routes: [route1, route2]\n" + "route1:\n" + " start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + " end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + "route2:\n" + " start: {x: 1.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + " end: {x: 2.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + + const auto filename = create_temp_yaml(yaml); + + // Declare parameters expected by RoutesMapsManager + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters({ + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 2u); + + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); + EXPECT_DOUBLE_EQ(routes[1].start.position.y, 1.0); + EXPECT_DOUBLE_EQ(routes[1].end.position.x, 2.0); +} + +TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_empty_path"); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters({ + rclcpp::Parameter("routes.map_path_file", std::string("")), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].start.position.y, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.y, 0.0); +} + +TEST_F(RoutesMapsManagerTest, DefaultRouteWhenYamlMissing) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_missing"); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("/tmp/non_existent_routes.yaml")); + + node->set_parameters({ + rclcpp::Parameter("routes.map_path_file", std::string("/tmp/non_existent_routes.yaml")), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); +} + +TEST_F(RoutesMapsManagerTest, DefaultRouteWhenNoRoutesKey) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_no_routes_key"); + + const std::string yaml = "foo: bar\n"; + const auto filename = create_temp_yaml(yaml); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters({ + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + const auto & routes = manager->get_routes(); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); +} + +TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node_update"); + + const std::string yaml = + "routes: [route1]\n" + "route1:\n" + " start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n" + " end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + + const auto filename = create_temp_yaml(yaml); + + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); + + node->set_parameters({ + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + easynav::TFInfo tf_info; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + ASSERT_NO_THROW(manager->initialize(node, "routes")); + + easynav::NavState nav_state; + manager->update(nav_state); + + ASSERT_TRUE(nav_state.has("routes")); + const auto & routes = nav_state.get("routes"); + ASSERT_EQ(routes.size(), 1u); + EXPECT_DOUBLE_EQ(routes[0].start.position.x, 0.0); + EXPECT_DOUBLE_EQ(routes[0].end.position.x, 1.0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt index 8d7be651..b47cd6cf 100644 --- a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 413125aa..4590daf1 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -1,23 +1,27 @@ # easynav_simple_maps_manager -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) - ## Description + Simple Maps Manager that demonstrates the Maps Manager API. It forwards/republishes a basic occupancy map flow and exposes standard topics and a save-map service. At the heart of this stack is the SimpleMap data structure. It represents the environment as a 2D occupancy grid where each cell can be either 0 (free), 1 (occupied), or -1 (unknown). ## Authors and Maintainers + - **Authors:** Intelligent Robotics Lab - **Maintainers:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_simple_maps_manager/SimpleMapsManager` - **Type:** `easynav::SimpleMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -25,38 +29,42 @@ At the heart of this stack is the SimpleMap data structure. It represents the en - **Description:** Simple (no-op) Maps Manager that demonstrates the Maps Manager API. It forwards/republishes a basic occupancy map flow and exposes standard topics and a save-map service. ## Parameters -### Plugin Parameters (namespace: `//easynav_simple_maps_manager/OctomapMapsManager/...`) + +### Plugin Parameters (namespace: `//easynav_simple_maps_manager/SimpleMapsManager/...`) + | Name | Type | Default | Description | |---|---|---:|---| | `.package` | `string` | `""` | Package name used to resolve relative map paths via `ament_index`. | | `.map_path_file` | `string` | `""` | Relative path (inside the package) to a simple map. | - -### ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `//incoming_map` | `nav_msgs/msg/OccupancyGrid` | Incoming occupancy map (used to feed the dynamic map). | QoS depth=1, transient_local, reliable | | Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Published static map. | QoS depth=1 | | Publisher | `//dynamic_map` | `nav_msgs/msg/OccupancyGrid` | Published dynamic (live) map. | QoS depth=100 | - ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves current map(s) to disk. | - ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Optional perception points bundle (not strictly required for this no-op manager). | - +| `map.static` | `SimpleMap` | **Write** | Static map loaded from file / parameter configuration. | +| `map.dynamic` | `SimpleMap` | **Write** | Dynamic map after applying incoming updates. | ## TF Frames + This manager does not broadcast TF; messages are stamped directly with their frame as provided. ## License + GPL-3.0-only diff --git a/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp index ff543893..bd9a2037 100644 --- a/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp +++ b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp @@ -23,24 +23,12 @@ #ifndef EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ #define EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ -#include -#include -#include -#include -#include -#include - #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "easynav_simple_common/SimpleMap.hpp" -#include "yaets/tracing.hpp" - namespace easynav { @@ -70,9 +58,9 @@ class SimpleMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 410c8a33..411cdf8d 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -20,10 +20,7 @@ /// \file /// \brief Implementation of the SimpleMapsManager class. -#include - #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -52,7 +49,7 @@ SimpleMapsManager::~SimpleMapsManager() } -std::expected +void SimpleMapsManager::on_initialize() { auto node = get_node(); @@ -72,11 +69,11 @@ SimpleMapsManager::on_initialize() pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + "/" + map_path_file; } catch(ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (!static_map_.load_from_file(map_path_)) { - return std::unexpected("File [" + map_path_ + "] not found"); + throw std::runtime_error("File [" + map_path_ + "] not found"); } } @@ -87,15 +84,17 @@ SimpleMapsManager::on_initialize() dynamic_occ_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/dynamic_map", 100); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + incoming_map_sub_ = node->create_subscription( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), - [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { + [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { static_map_.from_occupancy_grid(*msg); dynamic_map_.from_occupancy_grid(*msg); static_map_.to_occupancy_grid(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = this->get_node()->now(); static_occ_pub_->publish(static_grid_msg_); @@ -103,7 +102,7 @@ SimpleMapsManager::on_initialize() savemap_srv_ = node->create_service( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/savemap", - [this]( + [&]( const std::shared_ptr request, std::shared_ptr response) { @@ -118,12 +117,10 @@ SimpleMapsManager::on_initialize() }); static_map_.to_occupancy_grid(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); - - return {}; } void @@ -152,10 +149,11 @@ SimpleMapsManager::update(NavState & nav_state) } const auto & perceptions = nav_state.get("points"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map_.resolution()) - .fuse(get_tf_prefix() + "map") + .fuse(tf_info.map_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); @@ -170,7 +168,7 @@ SimpleMapsManager::update(NavState & nav_state) nav_state.set("map.dynamic", dynamic_map_); dynamic_map_.to_occupancy_grid(dynamic_grid_msg_); - dynamic_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + dynamic_grid_msg_.header.frame_id = tf_info.map_frame; dynamic_grid_msg_.header.stamp = get_node()->now(); dynamic_occ_pub_->publish(dynamic_grid_msg_); } diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 858a99cb..5e5b1006 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -21,12 +21,12 @@ #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "tf2_ros/transform_listener.hpp" #include "std_srvs/srv/trigger.hpp" @@ -40,18 +40,6 @@ class SimpleMapsManagerTest : public ::testing::Test void SetUp() override { rclcpp::init(0, nullptr); - - easynav::NavState::register_printer( - [](const easynav::PointPerceptions & perceptions) { - std::ostringstream ret; - ret << "PointPerception " << perceptions.size() << " with:\n"; - for (const auto & perception : perceptions) { - ret << "\t[" << static_cast(perception.get()) << "] --> " - << perception->data.size() << " points in frame [" << perception->frame_id - << "] with ts " << perception->stamp.seconds() << "\n"; - } - return ret.str(); - }); } void TearDown() override @@ -66,6 +54,13 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) { auto node = std::make_shared("test_node"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test"); auto tf_buffer = easynav::RTTFBuffer::getInstance(node->get_clock()); @@ -124,6 +119,13 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) { auto node = std::make_shared("test_node2"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test2"); easynav::NavState navstate; @@ -136,7 +138,7 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; + grid.header.frame_id = tf_info.map_frame; grid.info.width = 10; grid.info.height = 10; grid.info.resolution = 0.2; @@ -167,6 +169,13 @@ TEST_F(SimpleMapsManagerTest, SavemapServiceWorks) { auto node = std::make_shared("test_savemap_node"); auto manager = std::make_shared(); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + tf_info.robot_frame = "world_footprint_base"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + manager->initialize(node, "test_savemap"); easynav::SimpleMap map_static; diff --git a/planners/easynav_costmap_planner/CMakeLists.txt b/planners/easynav_costmap_planner/CMakeLists.txt index 77ef543f..580b6d71 100644 --- a/planners/easynav_costmap_planner/CMakeLists.txt +++ b/planners/easynav_costmap_planner/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_costmap_common REQUIRED) diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index fff2666f..d21e367c 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -1,6 +1,6 @@ # easynav_costmap_planner -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) +[![ROS 2: humble](https://img.shields.io/badge/ROS%202-humble-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) ## Description A planner plugin implementing a standard **A\*** path planner over a `Costmap2D` representation. It reads the dynamic costmap, goal list, and robot pose from NavState and publishes a computed path as a `nav_msgs/Path` message. @@ -12,6 +12,8 @@ A planner plugin implementing a standard **A\*** path planner over a `Costmap2D` ## Supported ROS 2 Distributions | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | @@ -29,12 +31,54 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | Name | Type | Default | Description | |---|---|---:|---| -| `.cost_factor` | `double` | `2.0` | Scaling factor applied to costmap cell values to compute traversal costs. | +| `.cost_factor` | `double` | `2.0` | Scaling factor applied to costmap cell values to compute traversal costs. Higher values make high-cost cells much less attractive. | | `.inflation_penalty` | `double` | `5.0` | Extra penalty added to inflated/near-obstacle cells to push paths away from obstacles. | -| `.cost_axial` | `double` | `1.0` | Base movement cost for axial (N/E/S/W) steps. | -| `.cost_diagonal` | `double` | `1.41` | Base movement cost for diagonal steps (approx. √2). | +| `.heuristic_scale` | `double` | `1.0` | Scaling factor applied to the A* heuristic term. Controls the trade-off between following the cheapest route vs. going more directly. | | `.continuous_replan` | `bool` | `true` | If `true`, re-plans continuously as the map/goal changes; if `false`, plans once per request. | +### Effect of `cost_factor` + +The planner uses a per-step traversal cost: + +$$ +g_{\text{new}} = g_{\text{current}} + \text{step\_cost} \cdot \left(1 + \text{cost\_factor} \cdot \frac{\text{cell\_cost}}{\text{LETHAL\_OBSTACLE}}\right) +$$ + +- **Low `cost_factor` (e.g. 1–2):** + - Cell cost has a modest influence; the planner is closer to a pure geometric shortest-path search. + - Paths may cross moderately expensive regions if that keeps distance short. +- **High `cost_factor` (e.g. 5–10+):** + - High-cost cells become very unattractive, so the planner strongly prefers low-cost corridors (e.g. routes or low-inflation areas), even if they are longer. + - Useful when combined with modules that encode preferences as higher costs (such as route managers). + +### Effect of `heuristic_scale` + +The A* priority is: + +$$ +f = g + h, \quad h = \text{heuristic\_scale} \cdot d_{\text{cells}}(\text{current}, \text{goal}) +$$ + +where $d_{\text{cells}}$ is the Euclidean distance in cell indices (not metric distance, but proportional). + +- **Decrease `heuristic_scale` (< 1.0):** + - The heuristic term becomes weaker, and A* behaves more like Dijkstra. + - The search explores more alternatives and is guided more strictly by the true accumulated cost $g$. + - This usually makes the planner follow high-cost penalties (e.g. for leaving a route) more faithfully, at the expense of more computation. + +- **Increase `heuristic_scale` (> 1.0):** + - The heuristic term dominates more, so the planner prefers geometrically direct paths. + - It may still avoid extremely high-cost regions if `cost_factor` is large, but will be more willing to “cut corners” through slightly more expensive zones. + +**Typical tuning strategy:** + +- First, adjust `cost_factor` so that the costmap properly encodes your preferences: + - Increase it until paths clearly avoid high-cost areas that should be disfavored. +- Then, fine-tune `heuristic_scale`: + - Start from `1.0`. + - If the planner still takes shortcuts that ignore cost differences, consider reducing it slightly (e.g. `0.7–0.9`). + - If planning becomes too slow or explores too widely, you can increase it modestly (e.g. `1.2–1.5`). + --- ## Interfaces (Topics and Services) diff --git a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp index c1bd7aee..0b13806b 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -23,7 +23,6 @@ #ifndef EASYNAV_COSTMAP_PLANNER__COSTMAPPLANNER_HPP_ #define EASYNAV_COSTMAP_PLANNER__COSTMAPPLANNER_HPP_ -#include #include #include "nav_msgs/msg/path.hpp" @@ -55,9 +54,9 @@ class CostmapPlanner : public PlannerMethodBase * Loads planner parameters, sets up ROS publishers, * and prepares the costmap-based planning environment. * - * @return std::expected A success indicator or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. @@ -71,8 +70,7 @@ class CostmapPlanner : public PlannerMethodBase protected: double cost_factor_; ///< Scaling factor applied to cell cost values. double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. - double cost_axial_; ///< Cost multiplier for axial (horizontal/vertical) moves. - double cost_diagonal_; ///< Cost multiplier for diagonal moves. + double heuristic_scale_ {1.0}; ///< Scaling factor applied to the heuristic term in A*. bool continuous_replan_ {true}; ///< Wheter replan path at freq time nav_msgs::msg::Path current_path_; ///< Most recently computed path. geometry_msgs::msg::Pose current_goal_; ///< Current goal. diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 916c6523..2f9b356c 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -23,10 +23,10 @@ #include #include #include -#include #include #include "easynav_costmap_planner/CostmapPlanner.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -69,57 +69,108 @@ static double compute_path_length(const nav_msgs::msg::Path & path) return total_length; } +// Simple path smoother: moving average over a sliding window in XY. +// Keeps endpoints unchanged to preserve exact start and goal. +static void smooth_path(std::vector & poses, int window_size = 5) +{ + if (poses.size() < 3 || window_size <= 1) { + return; + } + + // Ensure window_size is odd and at least 3 + if (window_size < 3) { + window_size = 3; + } + if (window_size % 2 == 0) { + window_size += 1; + } + + const int half = window_size / 2; + const size_t n = poses.size(); + std::vector original = poses; + + // Leave first and last pose untouched + for (size_t i = 1; i + 1 < n; ++i) { + double sum_x = 0.0; + double sum_y = 0.0; + int count = 0; + + const int begin = static_cast(std::max(0, + i > static_cast(half) ? i - half : 0)); + const int end = static_cast(std::min(n - 1, i + half)); + + for (int j = begin; j <= end; ++j) { + sum_x += original[j].position.x; + sum_y += original[j].position.y; + ++count; + } + + if (count > 0) { + poses[i].position.x = sum_x / static_cast(count); + poses[i].position.y = sum_y / static_cast(count); + } + } +} + CostmapPlanner::CostmapPlanner() { NavState::register_printer( [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length " + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); } -std::expected CostmapPlanner::on_initialize() +void CostmapPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); node->declare_parameter(plugin_name + ".cost_factor", 2.0); node->declare_parameter(plugin_name + ".inflation_penalty", 5.0); - node->declare_parameter(plugin_name + ".cost_axial", 1.0); - node->declare_parameter(plugin_name + ".cost_diagonal", 1.41); + node->declare_parameter(plugin_name + ".heuristic_scale", 1.0); node->declare_parameter(plugin_name + ".continuous_replan", true); node->get_parameter(plugin_name + ".cost_factor", cost_factor_); node->get_parameter(plugin_name + ".inflation_penalty", inflation_penalty_); - node->get_parameter(plugin_name + ".cost_axial", cost_axial_); - node->get_parameter(plugin_name + ".cost_diagonal", cost_diagonal_); - node->get_parameter(plugin_name + ".cost_diagonal", cost_diagonal_); + node->get_parameter(plugin_name + ".heuristic_scale", heuristic_scale_); node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); path_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; } void CostmapPlanner::update(NavState & nav_state) { - current_path_.poses.clear(); if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { return; } - const auto goals = nav_state.get("goals"); + const auto & goals = nav_state.get("goals"); if (goals.goals.empty()) { nav_state.set("path", current_path_); return; } const auto & map = nav_state.get("map.dynamic"); - const auto robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + rclcpp::Time latest_stamp = nav_state.get("map_time"); + if (rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()) > latest_stamp) { + latest_stamp = robot_pose.header.stamp; + } + if (rclcpp::Time(goals.goals.front().header.stamp, + latest_stamp.get_clock_type()) > latest_stamp) + { + latest_stamp = goals.goals.front().header.stamp; + } + current_path_.header.stamp = latest_stamp; - if (goals.header.frame_id != get_tf_prefix() + "map") { + if (goals.header.frame_id != tf_info.map_frame) { RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s", goals.header.frame_id.c_str()); return; @@ -142,8 +193,45 @@ void CostmapPlanner::update(NavState & nav_state) current_goal_ = goal; + // Lightweight skip: if inputs unchanged, avoid recomputation for a short window + static int last_sx = -1; + static int last_sy = -1; + static geometry_msgs::msg::Pose last_goal_pose; + static rclcpp::Time last_plan_time; + + unsigned int sx_chk, sy_chk; + if (map.worldToMap(robot_pose.pose.pose.position.x, robot_pose.pose.pose.position.y, sx_chk, + sy_chk)) + { + const bool same_start_cell = (static_cast(sx_chk) == last_sx) && + (static_cast(sy_chk) == last_sy); + const bool same_goal_pose = ( + std::fabs(goal.position.x - last_goal_pose.position.x) < 1e-6 && + std::fabs(goal.position.y - last_goal_pose.position.y) < 1e-6 && + goal.orientation.x == last_goal_pose.orientation.x && + goal.orientation.y == last_goal_pose.orientation.y && + goal.orientation.z == last_goal_pose.orientation.z && + goal.orientation.w == last_goal_pose.orientation.w); + + // Initialize last_plan_time on first use to current node time + if (last_plan_time.nanoseconds() == 0) { + last_plan_time = get_node()->now(); + } + const double since_last = (get_node()->now() - last_plan_time).seconds(); + // Only allow skipping when continuous_replan_ is disabled (event-based planning) + if (!continuous_replan_ && same_start_cell && same_goal_pose && since_last < 0.05) { + // Skip re-planning when nothing relevant changed recently + nav_state.set("path", current_path_); + return; + } + } + auto poses = a_star_path(map, robot_pose.pose.pose, goal); if (!poses.empty()) { + // Apply a light smoothing to the raw grid path + smooth_path(poses); + + current_path_.poses.clear(); current_path_.header.stamp = get_node()->now(); current_path_.header.frame_id = goals.header.frame_id; for (const auto & pose : poses) { @@ -153,9 +241,18 @@ void CostmapPlanner::update(NavState & nav_state) pose_stamped.pose = pose; current_path_.poses.push_back(pose_stamped); } + // Always publish a newly computed path if (path_pub_->get_subscription_count() > 0) { path_pub_->publish(current_path_); } + // Update last inputs snapshot + unsigned int sx, sy; + if (map.worldToMap(robot_pose.pose.pose.position.x, robot_pose.pose.pose.position.y, sx, sy)) { + last_sx = static_cast(sx); + last_sy = static_cast(sy); + } + last_goal_pose = goal; + last_plan_time = get_node()->now(); } nav_state.set("path", current_path_); } @@ -170,13 +267,23 @@ std::vector CostmapPlanner::a_star_path( if (!map.worldToMap(goal.position.x, goal.position.y, gx, gy)) {return {};} int width = map.getSizeInCellsX(); + // Precompute constants used inside the neighbor loop + const double lethal_norm = 1.0 / static_cast(LETHAL_OBSTACLE); + const double axial_cost = 1.0; + const double diagonal_cost = std::sqrt(2.0); auto idx = [&](int x, int y) {return y * width + x;}; std::priority_queue, std::greater<>> open; - std::unordered_map> came_from; - std::unordered_map cost_so_far; - open.push(GridNode{static_cast(sx), static_cast(sy), 0.0, heuristic(sx, sy, gx, gy)}); + const int height = map.getSizeInCellsY(); + const int total_cells = width * height; + std::vector parent_x(total_cells, -1); + std::vector parent_y(total_cells, -1); + std::vector cost_so_far(total_cells, std::numeric_limits::infinity()); + + const double initial_h = heuristic(static_cast(sx), static_cast(sy), + static_cast(gx), static_cast(gy)) * heuristic_scale_; + open.push(GridNode{static_cast(sx), static_cast(sy), 0.0, initial_h}); cost_so_far[idx(sx, sy)] = 0.0; while (!open.empty()) { @@ -190,30 +297,30 @@ std::vector CostmapPlanner::a_star_path( if (!map.inBounds(nx, ny)) {continue;} uint8_t cell_cost = map.getCost(nx, ny); - if (cell_cost >= LETHAL_OBSTACLE) {continue;} + // Reject cells that would cause collision (>= INSCRIBED_INFLATED_OBSTACLE = 253) + if (cell_cost >= INSCRIBED_INFLATED_OBSTACLE) {continue;} - double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost) / LETHAL_OBSTACLE; - if (cell_cost >= INSCRIBED_INFLATED_OBSTACLE) { - double inflation_ratio = static_cast(cell_cost - INSCRIBED_INFLATED_OBSTACLE) / - (LETHAL_OBSTACLE - INSCRIBED_INFLATED_OBSTACLE); - traversal_cost += inflation_penalty_ * inflation_ratio; - } + // Calculate traversal cost for free and lightly inflated cells + double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost) * lethal_norm; - double step_cost = (dx == 0 || dy == 0) ? cost_axial_ : cost_diagonal_; + double step_cost = (dx == 0 || dy == 0) ? axial_cost : diagonal_cost; double new_cost = cost_so_far[idx(current.x, current.y)] + traversal_cost * step_cost; int nid = idx(nx, ny); - if (!cost_so_far.contains(nid) || new_cost < cost_so_far[nid]) { + if (new_cost < cost_so_far[nid]) { cost_so_far[nid] = new_cost; - open.push(GridNode{nx, ny, new_cost, new_cost + heuristic(nx, ny, gx, gy)}); - came_from[nid] = {current.x, current.y}; + const double h = heuristic(nx, ny, static_cast(gx), static_cast(gy)) * + heuristic_scale_; + open.push(GridNode{nx, ny, new_cost, new_cost + h}); + parent_x[nid] = current.x; + parent_y[nid] = current.y; } } } std::vector path; int cx = static_cast(gx), cy = static_cast(gy); - while (came_from.contains(idx(cx, cy))) { + while (parent_x[idx(cx, cy)] != -1) { double wx, wy; map.mapToWorld(cx, cy, wx, wy); geometry_msgs::msg::Pose pose; @@ -221,7 +328,10 @@ std::vector CostmapPlanner::a_star_path( pose.position.y = wy; pose.orientation = goal.orientation; path.push_back(pose); - std::tie(cx, cy) = came_from[idx(cx, cy)]; + int px = parent_x[idx(cx, cy)]; + int py = parent_y[idx(cx, cy)]; + cx = px; + cy = py; } std::reverse(path.begin(), path.end()); diff --git a/planners/easynav_navmap_planner/CMakeLists.txt b/planners/easynav_navmap_planner/CMakeLists.txt index 950f854d..6e0ec154 100644 --- a/planners/easynav_navmap_planner/CMakeLists.txt +++ b/planners/easynav_navmap_planner/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_core REQUIRED) diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 58d80e1b..8e48b74e 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -1,9 +1,29 @@ # easynav_navmap_planner -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) +[![ROS 2: humble](https://img.shields.io/badge/ROS%202-humble-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) ## Description -A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goals from NavState and publishes a `nav_msgs/Path`. +**A\*** path planner operating over a `NavMap` triangular mesh. +The planner computes the **minimum-cost path** between the robot and the goal, taking into account both the geometric distance between triangles and the cost values stored in a selected NavMap layer (typically `"inflated_obstacles"`). + +Instead of simply avoiding non-free NavCels, the planner integrates their cost values (0–255) into the path evaluation. Cells marked as `LETHAL_OBSTACLE` or `NO_INFORMATION` are considered non-traversable, while inflated or inscribed cells are allowed but penalized proportionally to their cost. + +This enables smoother and safer trajectories that still respect proximity constraints imposed by obstacle inflation. + + +### Cost model +For two neighboring NavCels `u` and `v`, the edge cost is computed as: + +\[ +\text{cost}(u,v) = d(u,v) \times \left(\text{cost\_factor} + \text{inflation\_penalty} \times \frac{c(v)}{253}\right) +\] + +where `d(u,v)` is the Euclidean distance between triangle centroids, +and `c(v)` is the cost value of cell `v`. +This formulation ensures that: +- cells near obstacles (high cost) are traversed only if geometrically necessary, +- lethal (`254`) and unknown (`255`) cells are not traversable. + ## Authors and Maintainers - **Authors:** Intelligent Robotics Lab @@ -12,6 +32,8 @@ A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goal ## Supported ROS 2 Distributions | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | @@ -20,18 +42,18 @@ A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goal - **Type:** `easynav::navmap::AStarPlanner` - **Base Class:** `easynav::PlannerMethodBase` - **Library:** `easynav_navmap_planner` -- **Description:** A* path planner over a NavMap triangular surface/layer. Consumes NavMap and goals from NavState and publishes a `nav_msgs/Path`. +- **Description:** A\* path planner over a NavMap triangular mesh using per-cell costs to compute the shortest safe path. ## Parameters -All parameters are declared under the plugin namespace, i.e., `//easynav_navmap_planner/AStarPlanner/...`. +All parameters are declared under the plugin namespace, i.e. +`//easynav_navmap_planner/AStarPlanner/...` | Name | Type | Default | Description | |---|---|---:|---| -| `.layer` | `string` | `"inflated_obstacles"` | NavMap layer name to read costs from (e.g., `inflated_obstacles`). | -| `.cost_factor` | `double` | `2.0` | Scaling factor applied to cell/triangle costs. | -| `.inflation_penalty` | `double` | `5.0` | Extra penalty near inflated/inscribed regions to keep paths away from obstacles. | -| `.continuous_replan` | `bool` | `true` | Replan continuously as NavState updates (true) or plan once per request (false). | +| `.cost_factor` | `double` | `2.0` | Multiplicative weight for geometric distance; values > 1 increase the relative importance of distance. | +| `.continuous_replan` | `bool` | `true` | If true, recomputes the path whenever `NavState` updates; if false, plans once per goal. | +**Note:** The planner internally uses hardcoded values for `layer_name` (prefers `"inflated_obstacles"`, fallback to `"obstacles"`), `inflation_penalty` (value used in cost calculation), `cost_axial`, and `cost_diagonal`. These are not runtime-configurable parameters. ## Interfaces (Topics and Services) @@ -40,20 +62,18 @@ All parameters are declared under the plugin namespace, i.e., `//easyn |---|---|---|---|---| | Publisher | `//path` | `nav_msgs/msg/Path` | Publishes the computed A* path. | depth=10 | - -This plugin does not create subscriptions or services directly; it reads inputs from `NavState`. +This plugin does not create subscriptions or services directly; it retrieves all inputs from `NavState`. ## NavState Keys -| Key | Type | Access | Notes | +| Key | Type | Access | Description | |---|---|---|---| -| `goals` | `nav_msgs::msg::Goals` | **Read** | Planner targets. | -| `map` | `::navmap::NavMap` | **Read** | NavMap (reads the specified `layer`). | -| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Start pose for path planning. | -| `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | - +| `goals` | `nav_msgs::msg::Goals` | **Read** | Target pose(s) for path planning. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose (start position). | +| `map.navmap` | `::navmap::NavMap` | **Read** | NavMap containing geometry and cost layer. The planner reads costs from the layer specified in `.layer` (default: `"inflated_obstacles"`). If that layer does not exist, it automatically falls back to `"obstacles"`. | +| `path` | `nav_msgs::msg::Path` | **Write** | Output path, computed as the lowest-cost route. | ## TF Frames -This plugin does not perform TF lookups directly; frame consistency is assumed between NavMap, robot pose, and published path. +The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally. ## License GPL-3.0-only diff --git a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp index 09577ffc..ce01f50c 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -23,8 +23,9 @@ #ifndef EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ #define EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ -#include #include +#include +#include #include "nav_msgs/msg/path.hpp" @@ -39,7 +40,7 @@ namespace navmap /// \brief A planner implementing the A* algorithm on a ::navmap::NavMap grid. /// -/// This class generates a collision-free path using A* search over a 2D costmap. +/// This class generates a collision-free path using A* search over a surface-based NavMap. /// It supports cost-based penalties and anisotropic movement costs. class AStarPlanner : public PlannerMethodBase { @@ -55,11 +56,11 @@ class AStarPlanner : public PlannerMethodBase * @brief Initializes the planner. * * Loads planner parameters, sets up ROS publishers, - * and prepares the costmap-based planning environment. + * and prepares the NavMap-based planning environment. * - * @return std::expected A success indicator or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. @@ -72,17 +73,37 @@ class AStarPlanner : public PlannerMethodBase protected: double cost_factor_; ///< Scaling factor applied to cell cost values. - double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. - double cost_axial_; ///< Cost multiplier for axial (horizontal/vertical) moves. - double cost_diagonal_; ///< Cost multiplier for diagonal moves. + double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. + double cost_axial_; ///< Cost multiplier for axial (horizontal/vertical) moves. + double cost_diagonal_; ///< Cost multiplier for diagonal moves. std::string layer_name_; - bool continuous_replan_ {true}; ///< Wheter replan path at freq time + bool continuous_replan_ {true}; ///< Whether to replan the path at control frequency. nav_msgs::msg::Path current_path_; ///< Most recently computed path. geometry_msgs::msg::Pose current_goal_; ///< Current goal. /// Publisher for the computed navigation path (for visualization or monitoring). rclcpp::Publisher::SharedPtr path_pub_; + /// Cached centroids for each NavCel (same indexing as ::navmap::NavMap::navcels). + std::vector centroids_; + + /// Cached per-NavCel occupancy / cost values (0..255). + std::vector occ_; + + /// Reusable buffers for A* search cost and parent links. + std::vector g_; + std::vector<::navmap::NavCelId> parent_; + + /** + * @brief Ensure internal caches (centroids and A* buffers) are sized for the given map. + * + * This avoids reallocations on every planning call. Values in g_ and parent_ + * are reset for the current run. + * + * @param map The NavMap for which caches must be valid. + */ + void ensure_graph_cache(const ::navmap::NavMap & map); + /** * @brief Smooth a Path in XY while keeping every waypoint inside its original NavCel. * @@ -114,27 +135,21 @@ class AStarPlanner : public PlannerMethodBase /** * @brief Internal A* path planning routine. * - * Computes a path on the given costmap from the start pose to the goal pose. + * Computes a path on the given NavMap from the start pose to the goal pose. * * Movement cost is influenced by: - * - The cost of each cell (retrieved from the costmap). + * - The cost of each NavCel (retrieved from a layer). * - Additional inflation penalties near obstacles. - * - Anisotropic weights for axial vs diagonal movement. * - * @param map The costmap to plan over. + * @param map The NavMap to plan over. * @param start The robot's starting pose in world coordinates. - * @param goal The goal pose in world coordinates. + * @param goal The goal pose in world coordinates. * @return A vector of poses representing the planned path. */ std::vector a_star_path( const ::navmap::NavMap & map, const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal); - - /** - * @brief Internal static map. - */ - ::navmap::NavMap navmap_; }; } // namespace navmap diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 6f2ae5cb..0f1e7118 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -21,32 +21,25 @@ /// \brief Implementation of the AStarPlanner class using A* on ::navmap::NavMap (triangle graph). #include -#include -#include #include #include -#include -#include #include +#include +#include "easynav_common/RTTFBuffer.hpp" #include "easynav_navmap_planner/AStarPlanner.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" namespace easynav { namespace navmap { -static constexpr uint8_t NO_INFORMATION = 255; -static constexpr uint8_t LETHAL_OBSTACLE = 254; -static constexpr uint8_t INSCRIBED_INFLATED_OBSTACLE = 253; -static constexpr uint8_t MAX_NON_OBSTACLE = 252; -static constexpr uint8_t FREE_SPACE = 0; - static double compute_path_length(const nav_msgs::msg::Path & path) { double total_length = 0.0; @@ -63,28 +56,26 @@ AStarPlanner::AStarPlanner() NavState::register_printer( [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length " + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); } -std::expected AStarPlanner::on_initialize() +void AStarPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); node->declare_parameter(plugin_name + ".cost_factor", 2.0); - node->declare_parameter(plugin_name + ".inflation_penalty", 5.0); node->declare_parameter(plugin_name + ".continuous_replan", true); node->get_parameter(plugin_name + ".cost_factor", cost_factor_); - node->get_parameter(plugin_name + ".inflation_penalty", inflation_penalty_); node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); path_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; } void AStarPlanner::update(NavState & nav_state) @@ -94,21 +85,22 @@ void AStarPlanner::update(NavState & nav_state) return; } - - const auto goals = nav_state.get("goals"); - if (goals.goals.empty()) { + const auto & goals = nav_state.get("goals"); + if (goals.goals.empty() || !nav_state.has("map.navmap")) { nav_state.set("path", current_path_); return; } - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); - const auto robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (goals.header.frame_id != get_tf_prefix() + "map") { - RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s", - goals.header.frame_id.c_str()); + if (goals.header.frame_id != tf_info.map_frame) { + RCLCPP_WARN( + get_node()->get_logger(), "Goals frame is not 'map': %s", + goals.header.frame_id.c_str()); return; } @@ -121,7 +113,7 @@ void AStarPlanner::update(NavState & nav_state) } current_goal_ = goal; - auto poses = a_star_path(navmap_, robot_pose.pose.pose, goal); + auto poses = a_star_path(navmap, robot_pose.pose.pose, goal); if (!poses.empty()) { current_path_.header.stamp = get_node()->now(); current_path_.header.frame_id = goals.header.frame_id; @@ -134,7 +126,7 @@ void AStarPlanner::update(NavState & nav_state) current_path_.poses.push_back(std::move(ps)); } - current_path_ = path_smoother(current_path_, navmap_); + current_path_ = path_smoother(current_path_, navmap); if (path_pub_->get_subscription_count() > 0) { path_pub_->publish(current_path_); @@ -143,7 +135,6 @@ void AStarPlanner::update(NavState & nav_state) nav_state.set("path", current_path_); } - nav_msgs::msg::Path AStarPlanner::path_smoother( const nav_msgs::msg::Path & in_path, @@ -168,9 +159,10 @@ AStarPlanner::path_smoother( // Fill pts from input and locate cids for (size_t i = 0; i < N; ++i) { const auto & p = out.poses[i].pose.position; - pts[i] = Eigen::Vector3f(static_cast(p.x), - static_cast(p.y), - static_cast(p.z)); + pts[i] = Eigen::Vector3f( + static_cast(p.x), + static_cast(p.y), + static_cast(p.z)); } // Use walking hints to speed up sequential location @@ -206,7 +198,8 @@ AStarPlanner::path_smoother( } // Helper to fetch triangle vertices (A,B,C) for a cid - auto get_triangle_vertices = [&](::navmap::NavCelId cid) -> std::array { + auto get_triangle_vertices = + [&](::navmap::NavCelId cid) -> std::array { const ::navmap::NavCel & tri = navmap.navcels[cid]; const auto A = navmap.positions.at(tri.v[0]); const auto B = navmap.positions.at(tri.v[1]); @@ -215,8 +208,8 @@ AStarPlanner::path_smoother( }; // Helper: clamp a 3D point to the triangle of a given cid (closest point) - auto clamp_to_triangle = [&](const Eigen::Vector3f & p, - ::navmap::NavCelId cid) -> Eigen::Vector3f { + auto clamp_to_triangle = + [&](const Eigen::Vector3f & p, ::navmap::NavCelId cid) -> Eigen::Vector3f { const auto V = get_triangle_vertices(cid); return ::navmap::closest_point_on_triangle(p, V[0], V[1], V[2]); }; @@ -271,7 +264,7 @@ AStarPlanner::path_smoother( // Clamp to the *original* triangle of this point Eigen::Vector3f clamped = clamp_to_triangle(cand3, cids[i]); - next[i] = clamped; // already lies on triangle plane, z' consistent + next[i] = clamped; // already lies on triangle plane, z' consistent } curr.swap(next); } @@ -287,114 +280,221 @@ AStarPlanner::path_smoother( return out; } +// Helper: detect if a layer exists (optional; if you already have API, adjust accordingly) +static inline bool layer_exists(const ::navmap::NavMap & nm, const std::string & name) +{ + return static_cast(nm.layers.get(name)); +} + +void AStarPlanner::ensure_graph_cache(const ::navmap::NavMap & map) +{ + const std::size_t N = map.navcels.size(); + + // Cache centroids: only recompute when the number of NavCels changes. + if (centroids_.size() != N) { + centroids_.resize(N); + for (::navmap::NavCelId c = 0; c < static_cast<::navmap::NavCelId>(N); ++c) { + const auto cc = map.navcel_centroid(c); + centroids_[c] = Eigen::Vector3f{cc.x(), cc.y(), cc.z()}; + } + } + + // Ensure occupancy buffer has the right size (values are filled per-planning call). + if (occ_.size() != N) { + occ_.resize(N); + } + + // Resize and reset A* buffers. + const double inf = std::numeric_limits::infinity(); + + if (g_.size() != N) { + g_.assign(N, inf); + } else { + std::fill(g_.begin(), g_.end(), inf); + } + + if (parent_.size() != N) { + parent_.assign(N, std::numeric_limits<::navmap::NavCelId>::max()); + } else { + std::fill( + parent_.begin(), parent_.end(), + std::numeric_limits<::navmap::NavCelId>::max()); + } +} + std::vector AStarPlanner::a_star_path( const ::navmap::NavMap & nm, const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal) { using ::navmap::NavCelId; + using namespace navmap_ros; if (nm.navcels.empty()) {return {};} - // 1) Locate start & goal navcels (fallback to closest triangle) - size_t sidx_s = 0, sidx_g = 0; + // 1) Locate start and goal NavCels (fallback to closest triangle if necessary) + std::size_t sidx_s = 0, sidx_g = 0; NavCelId cid_start = 0, cid_goal = 0; - Eigen::Vector3f bary; Eigen::Vector3f hit; + Eigen::Vector3f bary; + Eigen::Vector3f hit; Eigen::Vector3f pS(start.position.x, start.position.y, start.position.z); Eigen::Vector3f pG(goal.position.x, goal.position.y, goal.position.z); bool okS = nm.locate_navcel(pS, sidx_s, cid_start, bary, &hit); if (!okS) { - Eigen::Vector3f q; float d2; + Eigen::Vector3f q; + float d2; if (!nm.closest_navcel(pS, sidx_s, cid_start, q, d2)) {return {};} } bool okG = nm.locate_navcel(pG, sidx_g, cid_goal, bary, &hit); if (!okG) { - Eigen::Vector3f q; float d2; + Eigen::Vector3f q; + float d2; if (!nm.closest_navcel(pG, sidx_g, cid_goal, q, d2)) {return {};} } - const size_t N = nm.navcels.size(); + const std::size_t N = nm.navcels.size(); - // 3) Precompute centroids (2D) for consistent metric and heuristic - std::vector C(N); - for (NavCelId c = 0; c < N; ++c) { - const auto cc = nm.navcel_centroid(c); - C[c] = {cc.x(), cc.y(), cc.z()}; - } + // 2) Choose cost layer: prefer "inflated_obstacles", fallback to "obstacles" + const std::string cost_layer = + layer_exists(nm, "inflated_obstacles") ? "inflated_obstacles" : "obstacles"; - auto h = [&](NavCelId a, NavCelId b) -> double { - const auto d = C[a] - C[b]; + // Ensure cached buffers match the current NavMap. + ensure_graph_cache(nm); + + // Precomputed centroids in `centroids_` are used for cost, heuristic, and edge lengths. + auto euclid = [&](NavCelId a, NavCelId b) -> double { + const auto d = centroids_[a] - centroids_[b]; return static_cast(d.norm()); }; + // Cache per-NavCel uint8_t cost values (0..255) for the selected layer. + for (NavCelId c = 0; c < static_cast(N); ++c) { + // If the cell has no stored value, assume FREE_SPACE (0). + occ_[c] = nm.layer_get(cost_layer, c, FREE_SPACE); + } + + // Traversability: block lethal and unknown. + auto traversable = [&](NavCelId c) -> bool { + const std::uint8_t v = occ_[c]; + return (v != LETHAL_OBSTACLE) && (v != NO_INFORMATION); + }; + + // Normalize a uint8_t cost into [0, 1]; FREE_SPACE=0 → 0.0; INSCRIBED=253 → ~1.0. + // Returns +inf for non-traversable (lethal/unknown). + auto normalized_cost = [&](NavCelId c) -> double { + const std::uint8_t v = occ_[c]; + if (v == LETHAL_OBSTACLE || v == NO_INFORMATION) { + return std::numeric_limits::infinity(); + } + // Cap at INSCRIBED_INFLATED_OBSTACLE to avoid division by 0 at 254/255. + const double max_cost = static_cast(INSCRIBED_INFLATED_OBSTACLE); // 253 + return static_cast(v) / max_cost; // FREE=0 → 0.0, INSCRIBED=253 → 1.0 + }; + + // If start or goal lands on non-traversable, do not plan. + if (!traversable(cid_start) || !traversable(cid_goal)) { + return {}; + } + + // Weighted step cost: + // base geometric cost (edge length) scaled by (cost_factor_ + inflation_penalty_ * norm_cost(target)). + // This preserves admissibility with heuristic h = Euclidean distance, since the minimal multiplier ≥ 1. auto step_cost = [&](NavCelId from, NavCelId to) -> double { - const double dist = static_cast((C[from] - C[to]).norm()); - return dist; + const double base = euclid(from, to); + if (!std::isfinite(base) || base <= 0.0) { + return std::numeric_limits::infinity(); + } + + const double ncost = normalized_cost(to); + if (!std::isfinite(ncost)) { + return std::numeric_limits::infinity(); + } + + // Ensure the multiplier is at least 1.0 so h = euclid remains admissible. + // If your cost_factor_ is already ≥ 1, this holds. Otherwise we clamp. + const double cf = std::max(1.0, static_cast(cost_factor_)); + const double mult = cf + static_cast(inflation_penalty_) * ncost; + + return base * mult; }; - // 4) A* on triangle graph - struct Node { NavCelId cid; double f; }; - struct Cmp { bool operator()(const Node & a, const Node & b) const {return a.f > b.f;} }; + // 4) A* search on the triangle graph, using cost-aware edges. + struct Node + { + NavCelId cid; + double f; + }; + struct Cmp + { + bool operator()(const Node & a, const Node & b) const {return a.f > b.f;} + }; std::priority_queue, Cmp> open; - std::vector g(N, std::numeric_limits::infinity()); - std::vector in_open(N, 0); - std::vector parent(N, std::numeric_limits::max()); - g[cid_start] = 0.0; + auto h = [&](NavCelId a, NavCelId b) -> double { + // Heuristic: pure Euclidean distance → admissible (never overestimates). + return euclid(a, b); + }; + + g_[cid_start] = 0.0; open.push(Node{cid_start, h(cid_start, cid_goal)}); - in_open[cid_start] = 1; while (!open.empty()) { - const auto cur = open.top(); open.pop(); + const auto cur = open.top(); + open.pop(); const NavCelId u = cur.cid; if (u == cid_goal) {break;} - // Optional: restrict to the goal surface (comment if you want cross-surface paths via explicit neighbors) - // const size_t surface_goal = sidx_g; - // if (surface_goal != sidx_s) { - // // do nothing special; graph neighbors already encode connectivity - // } + const auto & tri = nm.navcels[u]; + for (int e = 0; e < 3; ++e) { + NavCelId v = tri.neighbor[e]; + if (v == std::numeric_limits::max()) { + continue; + } + const std::size_t vidx = static_cast(v); + if (vidx >= N) { + continue; + } - for (NavCelId v : nm.navcel_neighbors(u)) { - const size_t vidx = static_cast(v); - if (vidx >= N) {continue;} + // Skip non-traversable neighbors (lethal or unknown). + if (!traversable(v)) {continue;} const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} - const double tentative = g[u] + sc; - if (tentative < g[v]) { - g[v] = tentative; - parent[v] = u; + const double tentative = g_[u] + sc; + if (tentative < g_[v]) { + g_[v] = tentative; + parent_[v] = u; const double f = tentative + h(v, cid_goal); open.push(Node{v, f}); - in_open[v] = 1; } } } - if (!std::isfinite(g[cid_goal])) { + if (!std::isfinite(g_[cid_goal])) { return {}; } - // 5) Reconstruct path (centroidal polyline) + // 5) Path reconstruction (centroid-based polyline). std::vector path; - for (NavCelId c = cid_goal; c != std::numeric_limits::max(); c = parent[c]) { + for (NavCelId c = cid_goal; + c != std::numeric_limits::max(); + c = parent_[c]) + { geometry_msgs::msg::Pose p; - p.position.x = C[c].x(); - p.position.y = C[c].y(); - p.position.z = C[c].z(); + p.position.x = centroids_[c].x(); + p.position.y = centroids_[c].y(); + p.position.z = centroids_[c].z(); p.orientation = goal.orientation; path.push_back(std::move(p)); if (c == cid_start) {break;} } std::reverse(path.begin(), path.end()); - // Ensure at least goal pose if (path.empty()) {path.push_back(goal);} return path; } diff --git a/planners/easynav_simple_planner/CMakeLists.txt b/planners/easynav_simple_planner/CMakeLists.txt index 5d6975ce..0282ad71 100644 --- a/planners/easynav_simple_planner/CMakeLists.txt +++ b/planners/easynav_simple_planner/CMakeLists.txt @@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(easynav_simple_common REQUIRED) diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index c19d8460..98335ace 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -1,6 +1,6 @@ # easynav_simple_planner -[![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) +[![ROS 2: humble](https://img.shields.io/badge/ROS%202-humble-blue)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) ## Description A simple A* planner over a lightweight `SimpleMap` grid. It reads the dynamic simple map, goals, and robot pose from NavState and publishes a `nav_msgs/Path`. @@ -12,6 +12,8 @@ A simple A* planner over a lightweight `SimpleMap` grid. It reads the dynamic si ## Supported ROS 2 Distributions | Distribution | Status | |---|---| +| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | | kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | diff --git a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp index 6dbb3253..48bda79e 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -23,7 +23,6 @@ #ifndef EASYNAV_PLANNER__SIMPLEPLANNER_HPP_ #define EASYNAV_PLANNER__SIMPLEPLANNER_HPP_ -#include #include #include "nav_msgs/msg/path.hpp" @@ -52,9 +51,9 @@ class SimplePlanner : public PlannerMethodBase * Configures publishers, retrieves parameters, and prepares the planner * for path generation using the available map data. * - * @return std::expected Success or an error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the planner by computing a new path. diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index bc9e8a77..7d116d20 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -20,9 +20,9 @@ #include #include #include -#include #include "easynav_simple_planner/SimplePlanner.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -79,14 +79,15 @@ SimplePlanner::SimplePlanner() [](const nav_msgs::msg::Path & path) { std::ostringstream ret; - ret << "Path with " << path.poses.size() << " poses and length" << + ret << "{ " << rclcpp::Time(path.header.stamp).seconds() << " } Path with " << + path.poses.size() << " poses and length " << compute_path_length(path) << " m."; return ret.str(); }); } -std::expected +void SimplePlanner::on_initialize() { auto node = get_node(); @@ -99,8 +100,6 @@ SimplePlanner::on_initialize() path_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - - return {}; } void @@ -111,7 +110,7 @@ SimplePlanner::update(NavState & nav_state) if (!nav_state.has("goals")) {return;} if (!nav_state.has("robot_pose")) {return;} - const auto goals = nav_state.get("goals"); + const auto & goals = nav_state.get("goals"); if (goals.goals.empty()) { nav_state.set("path", current_path_); @@ -131,12 +130,13 @@ SimplePlanner::update(NavState & nav_state) return; } - const auto robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); auto downsampled_map = map_typed.downsample(0.2); - if (goals.header.frame_id != "map") { + if (goals.header.frame_id != tf_info.map_frame) { RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update goals frame is not map (%s)", goals.header.frame_id.c_str()); return; @@ -246,7 +246,7 @@ SimplePlanner::a_star_path( double new_cost = cost_so_far[idx(current.x, current.y)] + hypot(dx, dy); int nid = idx(nx, ny); - if (!cost_so_far.contains(nid) || new_cost < cost_so_far[nid]) { + if (cost_so_far.find(nid) == cost_so_far.end() || new_cost < cost_so_far[nid]) { cost_so_far[nid] = new_cost; double priority = new_cost + heuristic(nx, ny, gx, gy); open.push({nx, ny, new_cost, priority}); @@ -257,7 +257,7 @@ SimplePlanner::a_star_path( std::vector path; int cx = gx, cy = gy; - while (came_from.contains(idx(cx, cy))) { + while (came_from.find(idx(cx, cy) ) != came_from.end()) { geometry_msgs::msg::Pose pose; auto [px, py] = map.cell_to_metric(cx, cy);