From e94b540324a779cdcad38738984bfd4b7649aa17 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 21 Oct 2025 07:26:36 +0200 Subject: [PATCH 001/136] Fix CI with the correct branch MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/jazzy.yaml | 55 ++++++++++++++++++++++++++++++++++ .github/workflows/kilted.yaml | 3 +- .github/workflows/rolling.yaml | 3 +- 3 files changed, 59 insertions(+), 2 deletions(-) create mode 100644 .github/workflows/jazzy.yaml diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml new file mode 100644 index 00000000..33198952 --- /dev/null +++ b/.github/workflows/jazzy.yaml @@ -0,0 +1,55 @@ +name: jazzy + +on: + pull_request: + branches: + - jazzy + push: + branches: + - jazzy + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + 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 + target-ros2-distro: jazzy + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: true + 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 efe7a6c7..f699529f 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -19,7 +19,8 @@ jobs: steps: - name: Repo checkout uses: actions/checkout@v4 - + with: + ref: kilted - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 with: diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 698e83e0..30cd778d 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -19,7 +19,8 @@ jobs: steps: - name: Repo checkout uses: actions/checkout@v4 - + with: + ref: rolling - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 with: From 75ba10254cf3213861632cac3caf927032ee7b33 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Oct 2025 07:59:44 +0200 Subject: [PATCH 002/136] Add params to navmap building map MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../NavMapMapsManager.hpp | 2 ++ .../NavMapMapsManager.cpp | 32 +++++++++++++++++-- 2 files changed, 32 insertions(+), 2 deletions(-) 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..9a217c2a 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 @@ -41,6 +41,7 @@ #include "easynav_core/MapsManagerBase.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" #include "pluginlib/class_loader.hpp" @@ -145,6 +146,7 @@ class NavMapMapsManager : public easynav::MapsManagerBase std::vector> navmap_filters_; + navmap_ros::BuildParams mapping_params_; double resolution_ {1.0}; }; 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..564dc2bb 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 @@ -69,10 +69,30 @@ NavMapMapsManager::on_initialize() node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->declare_parameter(plugin_name + ".navmap_path_file", navmap_path_file); + node->declare_parameter(plugin_name + ".resolution", mapping_params_.resolution); + node->declare_parameter(plugin_name + ".max_edge_len", mapping_params_.max_edge_len); + node->declare_parameter(plugin_name + ".neighbor_radius", mapping_params_.neighbor_radius); + node->declare_parameter(plugin_name + ".max_dz", mapping_params_.max_dz); + node->declare_parameter(plugin_name + ".max_slope_deg", mapping_params_.max_slope_deg); + node->declare_parameter(plugin_name + ".min_area", mapping_params_.min_area); + node->declare_parameter(plugin_name + ".max_surfaces", mapping_params_.max_surfaces); + node->declare_parameter(plugin_name + ".k_neighbors", mapping_params_.k_neighbors); + node->declare_parameter(plugin_name + ".min_angle_deg", mapping_params_.min_angle_deg); + node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->get_parameter(plugin_name + ".navmap_path_file", navmap_path_file); + node->get_parameter(plugin_name + ".resolution", mapping_params_.resolution); + node->get_parameter(plugin_name + ".max_edge_len", mapping_params_.max_edge_len); + node->get_parameter(plugin_name + ".neighbor_radius", mapping_params_.neighbor_radius); + node->get_parameter(plugin_name + ".max_dz", mapping_params_.max_dz); + node->get_parameter(plugin_name + ".max_slope_deg", mapping_params_.max_slope_deg); + node->get_parameter(plugin_name + ".min_area", mapping_params_.min_area); + node->get_parameter(plugin_name + ".max_surfaces", mapping_params_.max_surfaces); + node->get_parameter(plugin_name + ".k_neighbors", mapping_params_.k_neighbors); + node->get_parameter(plugin_name + ".min_angle_deg", mapping_params_.min_angle_deg); + std::vector navmap_filters; node->declare_parameter(plugin_name + ".filters", navmap_filters); node->get_parameter(plugin_name + ".filters", navmap_filters); @@ -177,11 +197,19 @@ 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); + params.resolution = 1.0f; + params.max_edge_len = 2.5f; + params.neighbor_radius = 2.5f; + params.max_dz = 0.4; + params.max_slope_deg = 30; + params.min_area = 0.25; + params.max_surfaces = 1; + navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); navmap_msg_.header.frame_id = "map"; navmap_msg_.header.stamp = this->get_node()->now(); From 0ea7c013e1f2f13f12a42d25ba827dea7c508bb7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 28 Oct 2025 14:09:11 +0800 Subject: [PATCH 003/136] Update README.md --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 5c9b0802..df009e83 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ # EasyNav Plugins [![Doxygen Deployment](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/doxygen-doc.yml/badge.svg)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/doxygen-doc.yml) -[![rolling](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/rolling.yaml/badge.svg)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/rolling.yaml) -[![kilted](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/kilted.yaml/badge.svg)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/kilted.yaml) -[![jazzy](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/jazzy.yaml/badge.svg)](https://github.com/EasyNavigation/easynav_plugins/actions/workflows/jazzy.yaml) +[![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) ## Description **EasyNav Plugins** provides the official collection of plugins for the [Easy Navigation (EasyNav)](https://github.com/EasyNavigation) framework. From 5043451653c38f0d66568d1ffab3ffea8bb5d58c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 1 Nov 2025 10:08:14 +0100 Subject: [PATCH 004/136] Merge from jazzy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/README.md | 4 +- .../easynav_serest_controller/README.md | 8 +- .../easynav_simple_controller/README.md | 81 ++++---- .../PIDController.hpp | 17 +- .../SimpleController.hpp | 9 +- .../PIDController.cpp | 63 +++--- .../SimpleController.cpp | 54 ++++- .../NavMapMapsManager.hpp | 2 - .../NavMapMapsManager.cpp | 32 +-- .../tests/simple_mapsmanager_tests.cpp | 2 +- .../easynav_navmap_planner/AStarPlanner.hpp | 28 +++ .../easynav_navmap_planner/AStarPlanner.cpp | 195 ++++++++++++++---- 12 files changed, 343 insertions(+), 152 deletions(-) diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index cd476102..655e8568 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -1,6 +1,7 @@ # easynav_mppi_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)](#) +[![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. @@ -14,6 +15,7 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig |---|---| | 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` diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index dfe76ab6..f5d6fa49 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -1,6 +1,7 @@ # easynav_serest_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)](#) +[![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 SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. @@ -11,9 +12,10 @@ A SeReST (Smooth Error-Responsive Speed and Turning) controller for path trackin ## 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) | +| 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` diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index 924f396f..c01e2eb2 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -1,62 +1,65 @@ -# easynav_vff_controller +# easynav_simple_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)](#) +[![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 -Vector Field Histogram (VFF) controller for obstacle avoidance and goal tracking. -Implements a reactive navigation approach that computes motion commands based on the vector sum of attractive and repulsive forces derived from goal and obstacle information. +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:** Jose Miguel Guerrero Hernández +- **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) | +| 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_vff_controller/VffController` -- **Type:** `easynav::VffController` -- **Base Class:** `easynav::ControllerMethodBase` -- **Library:** `vff_controller` -- **Description:** Reactive controller that generates velocity commands using the Vector Field Histogram method. +- **Plugin Name:** `easynav_simple_controller/SimpleController` +- **Type:** `easynav::SimpleController` +- **Base Class:** `easynav::ControllerMethodBase` +- **Library:** `easynav_simple_controller` +- **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_vff_controller/VffController/...`. +All parameters are declared under the plugin namespace, i.e., `//easynav_simple_controller/SimpleController/...`. | Name | Type | Default | Description | |---|---|---:|---| -| `.distance_obstacle_detection` | `float` | `3.0` | Distance threshold for obstacle detection. | -| `.distance_to_goal` | `float` | `1.0` | Minimum distance to consider the goal reached. | -| `.obstacle_detection_x_min` | `float` | `0.5` | Minimum X limit for obstacle detection (m). | -| `.obstacle_detection_x_max` | `float` | `10.0` | Maximum X limit for obstacle detection (m). | -| `.obstacle_detection_y_min` | `float` | `-10.0` | Minimum Y limit for obstacle detection (m). | -| `.obstacle_detection_y_max` | `float` | `10.0` | Maximum Y limit for obstacle detection (m). | -| `.obstacle_detection_z_min` | `float` | `0.10` | Minimum Z limit for obstacle detection (m). | -| `.obstacle_detection_z_max` | `float` | `1.00` | Maximum Z limit for obstacle detection (m). | -| `.max_speed` | `double` | `0.8` | Maximum linear velocity (m/s). | -| `.max_angular_speed` | `double` | `1.5` | Maximum angular velocity (rad/s). | - -## Interfaces (Topics and Services) - -### Subscriptions and Publications -This controller communicates exclusively through `NavState`; it does not create direct ROS 2 publishers or subscribers. - -### Services -This package does not create service servers or clients. - -## NavState Keys +| `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 | |---|---|---|---| -| `goals` | `nav_msgs::msg::Goals` | **Read** | Target goals to reach. | -| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose used to compute forces. | -| `points` | `PointPerceptions` | **Read** | Point cloud of obstacles. | -| `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Write** | Output velocity command computed by VFF. | +| `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. + ## TF Frames -This controller reads pose from `nav_msgs/Odometry` (`robot_pose` key). No TF lookups are performed. +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 +GPL-3.0-only \ No newline at end of file diff --git a/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp b/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp index f1f7bf80..7ab46f87 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp @@ -51,12 +51,18 @@ class PIDController /** * @brief Computes the control output for the given reference input. * - * Applies the PID formula and returns the control command within the specified output limits. - * - * @param new_reference The current reference input value (e.g., error signal). - * @return The computed control output. + * Applies the PID formula and returns the control command within the specified output limits. + * + * @param new_reference The current reference input value (e.g., error signal). + * @param dt Time elapsed since last call in seconds. + * @return The computed control output. */ - double get_output(double new_reference); + double get_output(double new_reference, double dt); + + /** + * @brief Reset internal integrator and derivative state. + */ + void reset(); private: double KP_; ///< Proportional gain. @@ -70,6 +76,7 @@ class PIDController double prev_error_; ///< Previous error value (for derivative term). double int_error_; ///< Accumulated integral error. + double integral_limit_ {0.0}; ///< If >0, clamps the integral term to ±integral_limit_. }; } // namespace easynav 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 61ff482b..e4cb072b 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp @@ -50,7 +50,14 @@ class SimpleController : public ControllerMethodBase double max_angular_acc_{0.3}; ///< Maximum angular acceleration in rad/s². double look_ahead_dist_{1.0}; ///< Distance ahead of the robot to track in meters. double tolerance_dist_{0.05}; ///< Distance threshold to switch to orientation tracking. - double k_rot_{0.5}; + double k_rot_{0.5}; ///< Gain to reduce linear speed based on angular velocity. + double final_goal_angle_tolerance_{0.1}; ///< Angular tolerance at the final goal in radians. + double linear_kp_{0.95}; ///< Proportional gain for linear PID. + double linear_ki_{0.03}; ///< Integral gain for linear PID. + double linear_kd_{0.08}; ///< Derivative gain for linear PID. + double angular_kp_{1.5}; ///< Proportional gain for angular PID. + double angular_ki_{0.03}; ///< Integral gain for angular PID. + double angular_kd_{0.08}; ///< Derivative gain for angular PID. double last_vlin_{0.0}; ///< Previous linear velocity for acceleration limiting. double last_vrot_{0.0}; ///< Previous angular velocity for acceleration limiting. diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp index 61dff23a..0526ee39 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp @@ -27,9 +27,10 @@ PIDController::PIDController(double min_ref, double max_ref, double min_output, max_output_ = max_output; prev_error_ = int_error_ = 0.0; - KP_ = 0.70; - KI_ = 0.15; - KD_ = 0.15; + KP_ = 0.7; + KI_ = 0.0; // start with no integral to avoid windup + KD_ = 0.0; + integral_limit_ = max_output_ * 2.0; // default integral clamp } void @@ -41,35 +42,51 @@ PIDController::set_pid(double n_KP, double n_KI, double n_KD) } double -PIDController::get_output(double new_reference) +PIDController::get_output(double new_reference, double dt) { - double ref = new_reference; - double output = 0.0; + if (dt <= 0.0) { + return 0.0; + } + + double error = new_reference; - // Proportional Error - double direction = 0.0; - if (ref != 0.0) { - direction = ref / fabs(ref); + // ignore very small errors + if (std::fabs(error) < min_ref_) { + // decay integrator slightly + int_error_ *= 0.9; + prev_error_ = error; + return 0.0; } - if (fabs(ref) < min_ref_) { - output = 0.0; - } else if (fabs(ref) > max_ref_) { - output = direction * max_output_; - } else { - output = direction * min_output_ + ref * (max_output_ - min_output_); + // Proportional term + double p = KP_ * error; + + // Integral term with anti-windup + int_error_ += error * dt; + if (integral_limit_ > 0.0) { + if (int_error_ * KI_ > integral_limit_) {int_error_ = integral_limit_ / KI_;} + if (int_error_ * KI_ < -integral_limit_) {int_error_ = -integral_limit_ / KI_;} } + double i = KI_ * int_error_; - // Integral Error - int_error_ = (int_error_ + output) * 2.0 / 3.0; + // Derivative term + double deriv = (error - prev_error_) / dt; + double d = KD_ * deriv; + prev_error_ = error; - // Derivative Error - double deriv_error = output - prev_error_; - prev_error_ = output; + double output = p + i + d; - output = KP_ * output + KI_ * int_error_ + KD_ * deriv_error; + // Saturate output to configured limits + output = std::clamp(output, -max_output_, max_output_); - return std::clamp(output, -max_output_, max_output_); + return output; +} + +void +PIDController::reset() +{ + prev_error_ = 0.0; + int_error_ = 0.0; } } // namespace easynav 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 e4da5d18..b89f2a88 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -51,6 +51,14 @@ SimpleController::on_initialize() node->declare_parameter(plugin_name + ".look_ahead_dist", look_ahead_dist_); node->declare_parameter(plugin_name + ".tolerance_dist", tolerance_dist_); node->declare_parameter(plugin_name + ".k_rot", k_rot_); + node->declare_parameter(plugin_name + ".final_goal_angle_tolerance", + final_goal_angle_tolerance_); + node->declare_parameter(plugin_name + ".linear_kp", linear_kp_); + node->declare_parameter(plugin_name + ".linear_ki", linear_ki_); + node->declare_parameter(plugin_name + ".linear_kd", linear_kd_); + node->declare_parameter(plugin_name + ".angular_kp", angular_kp_); + node->declare_parameter(plugin_name + ".angular_ki", angular_ki_); + node->declare_parameter(plugin_name + ".angular_kd", angular_kd_); node->get_parameter(plugin_name + ".max_linear_speed", max_linear_speed_); node->get_parameter(plugin_name + ".max_angular_speed", max_angular_speed_); @@ -59,10 +67,22 @@ SimpleController::on_initialize() node->get_parameter(plugin_name + ".look_ahead_dist", look_ahead_dist_); node->get_parameter(plugin_name + ".tolerance_dist", tolerance_dist_); node->get_parameter(plugin_name + ".k_rot", k_rot_); + node->get_parameter(plugin_name + ".final_goal_angle_tolerance", + final_goal_angle_tolerance_); + node->get_parameter(plugin_name + ".linear_kp", linear_kp_); + node->get_parameter(plugin_name + ".linear_ki", linear_ki_); + node->get_parameter(plugin_name + ".linear_kd", linear_kd_); + node->get_parameter(plugin_name + ".angular_kp", angular_kp_); + node->get_parameter(plugin_name + ".angular_ki", angular_ki_); + node->get_parameter(plugin_name + ".angular_kd", angular_kd_); linear_pid_ = std::make_shared(0.01, look_ahead_dist_, 0.1, max_linear_speed_); angular_pid_ = std::make_shared(0.01, M_PI, 0.1, max_angular_speed_); + // apply configured gains + linear_pid_->set_pid(linear_kp_, linear_ki_, linear_kd_); + angular_pid_->set_pid(angular_kp_, angular_ki_, angular_kd_); + last_vlin_ = 0.0; last_vrot_ = 0.0; last_update_ts_ = node->now(); @@ -79,7 +99,6 @@ SimpleController::update_rt(NavState & nav_state) if (!nav_state.has("path")) {return;} if (!nav_state.has("robot_pose")) {return;} - if (!nav_state.has("path")) {return;} const auto path = nav_state.get("path"); @@ -88,13 +107,38 @@ SimpleController::update_rt(NavState & nav_state) twist_stamped_.header.stamp = get_node()->now(); twist_stamped_.twist.linear.x = 0.0; twist_stamped_.twist.angular.z = 0.0; + // reset PID state when there's no path + if (linear_pid_) {linear_pid_->reset();} + if (angular_pid_) {angular_pid_->reset();} nav_state.set("cmd_vel", twist_stamped_); return; } - auto ref_pose = get_ref_pose(path, look_ahead_dist_); + // 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 & 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); + + if (dist_to_goal <= tolerance_dist_ && std::abs(angle_to_goal) <= final_goal_angle_tolerance_) { + // we're at the final goal -> publish zero velocity and reset integrators/state + last_vlin_ = 0.0; + last_vrot_ = 0.0; + twist_stamped_.header.frame_id = path.header.frame_id; + twist_stamped_.header.stamp = get_node()->now(); + twist_stamped_.twist.linear.x = 0.0; + twist_stamped_.twist.angular.z = 0.0; + // reset PID internal state to avoid windup / residual derivative + if (linear_pid_) {linear_pid_->reset();} + if (angular_pid_) {angular_pid_->reset();} + nav_state.set("cmd_vel", twist_stamped_); + RCLCPP_DEBUG(get_node()->get_logger(), + "%s: final goal reached (dist=%.3f, ang=%.3f), stopping.", + get_plugin_name().c_str(), dist_to_goal, angle_to_goal); + return; + } + auto ref_pose = get_ref_pose(path, look_ahead_dist_); double dist = get_distance(pose, ref_pose); @@ -108,10 +152,10 @@ SimpleController::update_rt(NavState & nav_state) if (dist < tolerance_dist_) { double diff_angle = get_diff_angle(pose.orientation, ref_pose.orientation); - vrot = angular_pid_->get_output(diff_angle); + vrot = angular_pid_->get_output(diff_angle, dt); } else { - vrot = angular_pid_->get_output(angle); - vlin = std::max(0.0, linear_pid_->get_output(dist) - k_rot_ * abs(vrot)); + vrot = angular_pid_->get_output(angle, dt); + vlin = std::max(0.0, linear_pid_->get_output(dist, dt) - k_rot_ * abs(vrot)); } 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 9a217c2a..c64609bf 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 @@ -41,7 +41,6 @@ #include "easynav_core/MapsManagerBase.hpp" #include "navmap_core/NavMap.hpp" -#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" #include "pluginlib/class_loader.hpp" @@ -146,7 +145,6 @@ class NavMapMapsManager : public easynav::MapsManagerBase std::vector> navmap_filters_; - navmap_ros::BuildParams mapping_params_; double resolution_ {1.0}; }; 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 564dc2bb..7725bcc9 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 @@ -69,30 +69,10 @@ NavMapMapsManager::on_initialize() node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->declare_parameter(plugin_name + ".navmap_path_file", navmap_path_file); - node->declare_parameter(plugin_name + ".resolution", mapping_params_.resolution); - node->declare_parameter(plugin_name + ".max_edge_len", mapping_params_.max_edge_len); - node->declare_parameter(plugin_name + ".neighbor_radius", mapping_params_.neighbor_radius); - node->declare_parameter(plugin_name + ".max_dz", mapping_params_.max_dz); - node->declare_parameter(plugin_name + ".max_slope_deg", mapping_params_.max_slope_deg); - node->declare_parameter(plugin_name + ".min_area", mapping_params_.min_area); - node->declare_parameter(plugin_name + ".max_surfaces", mapping_params_.max_surfaces); - node->declare_parameter(plugin_name + ".k_neighbors", mapping_params_.k_neighbors); - node->declare_parameter(plugin_name + ".min_angle_deg", mapping_params_.min_angle_deg); - node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->get_parameter(plugin_name + ".navmap_path_file", navmap_path_file); - node->get_parameter(plugin_name + ".resolution", mapping_params_.resolution); - node->get_parameter(plugin_name + ".max_edge_len", mapping_params_.max_edge_len); - node->get_parameter(plugin_name + ".neighbor_radius", mapping_params_.neighbor_radius); - node->get_parameter(plugin_name + ".max_dz", mapping_params_.max_dz); - node->get_parameter(plugin_name + ".max_slope_deg", mapping_params_.max_slope_deg); - node->get_parameter(plugin_name + ".min_area", mapping_params_.min_area); - node->get_parameter(plugin_name + ".max_surfaces", mapping_params_.max_surfaces); - node->get_parameter(plugin_name + ".k_neighbors", mapping_params_.k_neighbors); - node->get_parameter(plugin_name + ".min_angle_deg", mapping_params_.min_angle_deg); - std::vector navmap_filters; node->declare_parameter(plugin_name + ".filters", navmap_filters); node->get_parameter(plugin_name + ".filters", navmap_filters); @@ -197,20 +177,12 @@ 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), - [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { - + [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) { navmap_ros::BuildParams params; - params.resolution = 1.0f; - params.max_edge_len = 2.5f; - params.neighbor_radius = 2.5f; - params.max_dz = 0.4; - params.max_slope_deg = 30; - params.min_area = 0.25; - params.max_surfaces = 1; - navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); + navmap_msg_.header.frame_id = "map"; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_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 8da903ba..858a99cb 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 @@ -69,7 +69,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) manager->initialize(node, "test"); auto tf_buffer = easynav::RTTFBuffer::getInstance(node->get_clock()); - tf2_ros::TransformListener tf_listener(*tf_buffer, *node, true); + tf2_ros::TransformListener tf_listener(*tf_buffer, node, true); easynav::SimpleMap static_map; static_map.initialize(30, 30, 0.1, -1.5, -1.5, 0.0); 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 06eeae3c..09577ffc 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -83,6 +83,34 @@ class AStarPlanner : public PlannerMethodBase /// Publisher for the computed navigation path (for visualization or monitoring). rclcpp::Publisher::SharedPtr path_pub_; + /** + * @brief Smooth a Path in XY while keeping every waypoint inside its original NavCel. + * + * The algorithm performs several iterations of Laplacian smoothing on XY: + * p_i' = (1 - alpha) * p_i + alpha * 0.5 * (p_{i-1} + p_{i+1}) + * For each i, the candidate point is clamped to the original triangle (NavCel) + * using closest-point-on-triangle, so it never leaves that NavCel. The final z' + * is the triangle height at the resulting (x', y'). + * + * Endpoints are kept fixed. Optionally, points forming a sharp angle are also + * kept (see `corner_keep_deg`). + * + * @param in_path Input nav_msgs::msg::Path (world coordinates). + * @param navmap The NavMap where the path lies on. + * @param iterations Number of smoothing iterations (>= 1). Default: 5. + * @param alpha Smoothing factor in (0, 0.5]. Default: 0.4. + * @param corner_keep_deg Angle threshold (degrees): if the interior angle at a point + * is below this value, the point is kept as an anchor. Set <= 0 + * to disable. Default: 0 (disabled). + * @return nav_msgs::msg::Path Smoothed path, same frame_id and header stamp as input. + */ + nav_msgs::msg::Path path_smoother( + const nav_msgs::msg::Path & in_path, + const ::navmap::NavMap & navmap, + int iterations = 5, + float alpha = 0.4f, + float corner_keep_deg = 0.0f); + /** * @brief Internal A* path planning routine. * 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 e1967352..5a5b0c2c 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -74,12 +74,10 @@ std::expected AStarPlanner::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - node->declare_parameter(plugin_name + ".layer", "inflated_obstacles"); 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 + ".layer", layer_name_); 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_); @@ -92,17 +90,18 @@ std::expected AStarPlanner::on_initialize() void AStarPlanner::update(NavState & nav_state) { current_path_.poses.clear(); - if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map")) { + if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.navmap")) { return; } + const auto goals = nav_state.get("goals"); if (goals.goals.empty()) { nav_state.set("path", current_path_); return; } - navmap_ = nav_state.get<::navmap::NavMap>("map"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const auto robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; @@ -122,7 +121,6 @@ void AStarPlanner::update(NavState & nav_state) } current_goal_ = goal; - auto poses = a_star_path(navmap_, robot_pose.pose.pose, goal); if (!poses.empty()) { current_path_.header.stamp = get_node()->now(); @@ -135,6 +133,9 @@ void AStarPlanner::update(NavState & nav_state) ps.pose = pose; current_path_.poses.push_back(std::move(ps)); } + + current_path_ = path_smoother(current_path_, navmap_); + if (path_pub_->get_subscription_count() > 0) { path_pub_->publish(current_path_); } @@ -142,6 +143,149 @@ 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, + const ::navmap::NavMap & navmap, + int iterations, + float alpha, + float corner_keep_deg) +{ + nav_msgs::msg::Path out = in_path; + if (out.poses.size() < 3 || iterations <= 0 || alpha <= 0.0f) { + // Nothing to do + return out; + } + + const size_t N = out.poses.size(); + + // --- 1) Pre-locate the original NavCel for each point (and keep it fixed) --- + std::vector<::navmap::NavCelId> cids(N, std::numeric_limits::max()); + std::vector surf_idx(N, std::numeric_limits::max()); + std::vector pts(N); + + // 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)); + } + + // Use walking hints to speed up sequential location + ::navmap::NavMap::LocateOpts opts; + for (size_t i = 0; i < N; ++i) { + size_t sidx = 0; + ::navmap::NavCelId cid{}; + Eigen::Vector3f bary; + Eigen::Vector3f hit; + + // Try full locate with hint (from previous point) + bool ok = navmap.locate_navcel(pts[i], sidx, cid, bary, &hit, opts); + if (!ok) { + // Fallback to closest triangle (keeps the query on-surface) + float sqd = 0.0f; + Eigen::Vector3f cp; + ok = navmap.closest_navcel(pts[i], sidx, cid, cp, sqd); + if (ok) { + pts[i] = cp; + } + } + if (ok) { + surf_idx[i] = sidx; + cids[i] = cid; + opts.hint_cid = cid; + opts.hint_surface = sidx; + } else { + // If locate fails, keep the original point but mark cid as invalid. + cids[i] = std::numeric_limits::max(); + opts.hint_cid.reset(); + opts.hint_surface.reset(); + } + } + + // Helper to fetch triangle vertices (A,B,C) for a cid + 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]); + const auto C = navmap.positions.at(tri.v[2]); + return {A, B, C}; + }; + + // 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 { + const auto V = get_triangle_vertices(cid); + return ::navmap::closest_point_on_triangle(p, V[0], V[1], V[2]); + }; + + // Optional: precompute anchors for sharp corners + std::vector is_anchor(N, 0); + is_anchor.front() = 1; + is_anchor.back() = 1; + if (corner_keep_deg > 0.0f && N >= 3) { + const float thr_rad = corner_keep_deg * static_cast(M_PI) / 180.0f; + for (size_t i = 1; i + 1 < N; ++i) { + const Eigen::Vector2f a(pts[i - 1].x(), pts[i - 1].y()); + const Eigen::Vector2f b(pts[i].x(), pts[i].y()); + const Eigen::Vector2f c(pts[i + 1].x(), pts[i + 1].y()); + const Eigen::Vector2f u = (a - b); + const Eigen::Vector2f v = (c - b); + float nu = u.norm(), nv = v.norm(); + if (nu > 1e-6f && nv > 1e-6f) { + float cosang = u.dot(v) / (nu * nv); + cosang = std::max(-1.0f, std::min(1.0f, cosang)); + float ang = std::acos(cosang); + if (ang < thr_rad) {is_anchor[i] = 1;} + } + } + } + + // --- 2) Iterative smoothing with per-point (fixed) triangle constraint --- + std::vector curr = pts; + std::vector next = pts; + + for (int it = 0; it < iterations; ++it) { + for (size_t i = 0; i < N; ++i) { + // Keep invalid-cid points and anchors untouched + if (i == 0 || i == N - 1 || is_anchor[i] || + cids[i] == std::numeric_limits::max()) + { + next[i] = curr[i]; + continue; + } + + const Eigen::Vector2f prev_xy(curr[i - 1].x(), curr[i - 1].y()); + const Eigen::Vector2f curr_xy(curr[i].x(), curr[i].y()); + const Eigen::Vector2f next_xy(curr[i + 1].x(), curr[i + 1].y()); + + // Laplacian target in XY + const Eigen::Vector2f lap_target = 0.5f * (prev_xy + next_xy); + Eigen::Vector2f cand_xy = (1.0f - alpha) * curr_xy + alpha * lap_target; + + // Build a 3D candidate with current z as seed; then clamp to triangle + Eigen::Vector3f cand3(cand_xy.x(), cand_xy.y(), curr[i].z()); + + // 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 + } + curr.swap(next); + } + + // --- 3) Write back to Path (keeping header/frame) --- + for (size_t i = 0; i < N; ++i) { + out.poses[i].pose.position.x = curr[i].x(); + out.poses[i].pose.position.y = curr[i].y(); + out.poses[i].pose.position.z = curr[i].z(); + // Orientation: leave untouched; if needed, you can realign yaw to local tangent later. + } + + return out; +} + std::vector AStarPlanner::a_star_path( const ::navmap::NavMap & nm, const geometry_msgs::msg::Pose & start, @@ -170,32 +314,13 @@ std::vector AStarPlanner::a_star_path( if (!nm.closest_navcel(pG, sidx_g, cid_goal, q, d2)) {return {};} } - // 2) Choose layer to query - std::string layer = layer_name_; - if (!nm.has_layer(layer)) { - if (!nm.has_layer(layer)) { - RCLCPP_WARN(get_node()->get_logger(), "No layer '%s' nor 'obstacles' found", - layer_name_.c_str()); - return {}; - } - } - - // Impassable predicate - auto blocked = [&](NavCelId c) -> bool { - uint8_t v = nm.layer_get(layer, c, FREE_SPACE); - return (v == NO_INFORMATION) || (v >= LETHAL_OBSTACLE); - }; - if (blocked(cid_start) || blocked(cid_goal)) { - return {}; - } - const size_t N = nm.navcels.size(); // 3) Precompute centroids (2D) for consistent metric and heuristic - std::vector C(N); + std::vector C(N); for (NavCelId c = 0; c < N; ++c) { const auto cc = nm.navcel_centroid(c); - C[c] = {cc.x(), cc.y()}; + C[c] = {cc.x(), cc.y(), cc.z()}; } auto h = [&](NavCelId a, NavCelId b) -> double { @@ -205,20 +330,7 @@ std::vector AStarPlanner::a_star_path( auto step_cost = [&](NavCelId from, NavCelId to) -> double { const double dist = static_cast((C[from] - C[to]).norm()); - const uint8_t cost_to = nm.layer_get(layer, to, FREE_SPACE); - if (cost_to >= LETHAL_OBSTACLE || cost_to == NO_INFORMATION) { - return std::numeric_limits::infinity(); - } - const double norm = static_cast(cost_to) / static_cast(LETHAL_OBSTACLE); - double c = dist * (1.0 + cost_factor_ * norm); - if (cost_to >= INSCRIBED_INFLATED_OBSTACLE) { - // extra penalty near obstacles - const double ratio = - static_cast(cost_to - INSCRIBED_INFLATED_OBSTACLE) / - static_cast(LETHAL_OBSTACLE - INSCRIBED_INFLATED_OBSTACLE); - c += inflation_penalty_ * std::max(0.0, ratio); - } - return c; + return dist; }; // 4) A* on triangle graph @@ -249,7 +361,6 @@ std::vector AStarPlanner::a_star_path( for (NavCelId v : nm.navcel_neighbors(u)) { const size_t vidx = static_cast(v); if (vidx >= N) {continue;} - if (blocked(v)) {continue;} const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} @@ -275,7 +386,7 @@ std::vector AStarPlanner::a_star_path( geometry_msgs::msg::Pose p; p.position.x = C[c].x(); p.position.y = C[c].y(); - p.position.z = 0.0; + p.position.z = C[c].z(); p.orientation = goal.orientation; path.push_back(std::move(p)); if (c == cid_start) {break;} From 4b0ca8215cc0e0f0aa4df3938251ecaa1b7cb703 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 1 Nov 2025 10:12:37 +0100 Subject: [PATCH 005/136] Linter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_navmap_planner/AStarPlanner.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 5a5b0c2c..6f2ae5cb 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -215,7 +215,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]); }; From 6c2b851962732718784a1f29cd618cb12253e0de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 2 Nov 2025 11:03:19 +0100 Subject: [PATCH 006/136] Prepare for humble MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/humble.yaml | 55 +++++++++++++++++++++++++++++++++++ README.md | 1 + 2 files changed, 56 insertions(+) create mode 100644 .github/workflows/humble.yaml diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml new file mode 100644 index 00000000..6c2ee7ed --- /dev/null +++ b/.github/workflows/humble.yaml @@ -0,0 +1,55 @@ +name: humble + +on: + pull_request: + branches: + - humble + push: + branches: + - humble + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-22.04] + fail-fast: false + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: humble + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + with: + required-ros-distributions: humble + + - 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: true + 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/README.md b/README.md index df009e83..1f2c8957 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,7 @@ [![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) ## Description **EasyNav Plugins** provides the official collection of plugins for the [Easy Navigation (EasyNav)](https://github.com/EasyNavigation) framework. From 8f844abd92385ee2a6ba3bbff71d61f0e3befa32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 3 Nov 2025 08:49:09 +0100 Subject: [PATCH 007/136] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 1f2c8957..ea126c17 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,8 @@ [![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. From 8fc2aa078dff96e392f6d17e3a0ebdfe6ffc13e3 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 4 Nov 2025 08:14:29 +0100 Subject: [PATCH 008/136] Fix TF compilation warnings --- .../tests/simple_mapsmanager_tests.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..8da903ba 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 @@ -69,7 +69,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) manager->initialize(node, "test"); auto tf_buffer = easynav::RTTFBuffer::getInstance(node->get_clock()); - tf2_ros::TransformListener tf_listener(*tf_buffer, node, true); + tf2_ros::TransformListener tf_listener(*tf_buffer, *node, true); easynav::SimpleMap static_map; static_map.initialize(30, 30, 0.1, -1.5, -1.5, 0.0); From 195dfccb2bfb832c76813590ce2c95c0676533da Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Sat, 8 Nov 2025 10:30:40 +0100 Subject: [PATCH 009/136] Fix errors in periodic CI --- .github/workflows/humble.yaml | 1 + .github/workflows/jazzy.yaml | 1 + .github/workflows/kilted.yaml | 1 + .github/workflows/rolling.yaml | 1 + 4 files changed, 4 insertions(+) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 6c2ee7ed..656ea9de 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -31,6 +31,7 @@ jobs: 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: true colcon-defaults: | diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 33198952..fa433e5f 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -31,6 +31,7 @@ jobs: 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: jazzy + ref: jazzy vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: true colcon-defaults: | diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index f699529f..bcee3c4d 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -31,6 +31,7 @@ jobs: 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: kilted + ref: kilted vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: true colcon-defaults: | diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 30cd778d..651aa501 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -31,6 +31,7 @@ jobs: 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: rolling + ref: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: true colcon-defaults: | From b4fcc07cb92bfe0095d713b01e9c27598f45f58a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 10 Nov 2025 06:58:51 +0100 Subject: [PATCH 010/136] First functional verison, but expensive MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CMakeLists.txt | 4 +- .../filters/NavMapFilter.hpp | 5 +- .../filters/ObstacleFilter.hpp | 4 +- .../NavMapMapsManager.cpp | 56 +++++++++------- .../filters/ObstacleFilter.cpp | 67 ++++++++++++++----- 5 files changed, 85 insertions(+), 51 deletions(-) diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index 09006d4a..f1e05f04 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt @@ -90,8 +90,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/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..6733a9d0 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 @@ -53,11 +53,10 @@ 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; +protected: + std::shared_ptr get_node() const; const std::string & get_tf_prefix() const; protected: 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..38833490 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 @@ -44,8 +44,8 @@ class ObstacleFilter : public NavMapFilter virtual std::expected on_initialize(); virtual void update(::easynav::NavState & nav_state); - 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/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index 7725bcc9..a2e3f54f 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 @@ -25,6 +25,7 @@ #include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" +#include "easynav_common/plugin_diag.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -45,15 +46,23 @@ 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"); + + ::easynav::dump_pluginlib_diagnostics(*navmap_filters_loader_, std::cerr, /*try_load_each_class=*/false); } NavMapMapsManager::~NavMapMapsManager() {} @@ -85,7 +94,6 @@ 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); @@ -219,27 +227,23 @@ 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); -// } -// } + std::cerr << + "plugin name: " << filter->get_plugin_name() << + "\tlayer name: " << filter->get_layer_name() << + "\tis_adding" << (filter->is_adding_layer()? "True" : "False") << std::endl; + + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + + if (filter->is_adding_layer() && navmap_.has_layer(filter->get_layer_name())) { + std::cerr << "Publishin layer " << filter->get_layer_name() << std::endl; + 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/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index faec9835..af2c3293 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 @@ -49,7 +49,10 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { - if (!nav_state.has("map")) { + auto t0 = parent_node_->now(); + + std::cerr << "ObstacleFilter::update" << std::endl; + if (!nav_state.has("map.navmap")) { return; } if (!nav_state.has("points")) { @@ -57,35 +60,63 @@ ObstacleFilter::update(::easynav::NavState & nav_state) } const auto & perceptions = nav_state.get("points"); - navmap_ = nav_state.get<::navmap::NavMap>("map"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - if (!navmap_.layer_copy("occupancy", "obstacles")) { - RCLCPP_ERROR(parent_node_->get_logger(), "Error copying layers at ObstacleFilter"); - return; - } + navmap_.layer_clear(get_layer_name(), 0.0f); + + auto t1 = parent_node_->now(); auto fused = PointPerceptionsOpsView(perceptions) - .downsample(get_map_resolution()) + .downsample(0.1) .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) .as_points(); + auto t2 = parent_node_->now(); + size_t sidx = 0; + std::optional<::navmap::NavCelId> last_cid; + + auto t3 = parent_node_->now(); + for (const auto & p : fused) { - if (std::isnan(p.x)) {continue;} + if (std::isnan(p.x) || std::isinf(p.x)) {continue;} + ::navmap::NavCelId cid; - Eigen::Vector3f bary; - Eigen::Vector3f hit; - - 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); - } + Eigen::Vector3f bary, hit; + bool located = false; + + { + ::navmap::NavMap::LocateOpts opts; + if (last_cid) opts.hint_cid = *last_cid; + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); + } + + if (!located) { + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit); + if (!located) continue; + } + + last_cid = cid; + + const float h = static_cast(p.z) - hit.z(); + if ((h < 0.0f) || !std::isfinite(h)) continue; + + if (h > 0.1) { + navmap_.layer_set(get_layer_name(), cid, h); } } - nav_state.set("map", navmap_); + auto t4 = parent_node_->now(); + nav_state.set("map.navmap", navmap_); + + auto t5 = parent_node_->now(); + + std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; + std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; + std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; + std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; + std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } } // namespace navmap From be90b77bb66324d7ea1c4bb4bc0c9d8ff8616ec6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 10 Nov 2025 07:12:57 +0100 Subject: [PATCH 011/136] Optimized function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/ObstacleFilter.cpp | 110 +++++++++++++----- 1 file changed, 83 insertions(+), 27 deletions(-) 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 af2c3293..3d7be5d6 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 @@ -46,70 +46,125 @@ ObstacleFilter::on_initialize() return {}; } -void -ObstacleFilter::update(::easynav::NavState & nav_state) +void ObstacleFilter::update(::easynav::NavState & nav_state) { auto t0 = parent_node_->now(); - std::cerr << "ObstacleFilter::update" << std::endl; - if (!nav_state.has("map.navmap")) { - 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"); + // Si necesitas limpieza total en cada ciclo, mantén esto: navmap_.layer_clear(get_layer_name(), 0.0f); auto t1 = parent_node_->now(); auto fused = PointPerceptionsOpsView(perceptions) - .downsample(0.1) - .fuse(get_tf_prefix() + "map") - ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) - .as_points(); + .downsample(0.3) + .fuse(get_tf_prefix() + "map") + ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) + .as_points(); + + // (Opcional pero muy rentable) Orden espacial para mejorar locality del hint: + // ordenar por una clave Morton simple 2D (x,y). Implementación ligera: + auto morton2D = [](float x, float y) -> uint64_t { + // normaliza a rejilla 1 cm en [-64,64] m => 12800 pasos (cambia a tu rango) + auto q = [](float v){ + int32_t iv = static_cast(std::lrintf((v + 64.f) * 100.f)); + if (iv < 0) iv = 0; else if (iv > 12800) iv = 12800; + return static_cast(iv); + }; + auto part = [](uint32_t v){ + uint64_t x = v; + x = (x | (x << 16)) & 0x0000FFFF0000FFFFULL; + x = (x | (x << 8 )) & 0x00FF00FF00FF00FFULL; + x = (x | (x << 4 )) & 0x0F0F0F0F0F0F0F0FULL; + x = (x | (x << 2 )) & 0x3333333333333333ULL; + x = (x | (x << 1 )) & 0x5555555555555555ULL; + return x; + }; + uint64_t xi = part(q(x)); + uint64_t yi = part(q(y)) << 1; + return xi | yi; + }; + + std::sort(fused.begin(), fused.end(), + [&](const auto &a, const auto &b){ + return morton2D(a.x, a.y) < morton2D(b.x, b.y); + }); auto t2 = parent_node_->now(); - size_t sidx = 0; - std::optional<::navmap::NavCelId> last_cid; + // Acumulación de h máximos por celda (reduce layer_set) + struct NavCelIdHash { + std::size_t operator()(const ::navmap::NavCelId &c) const noexcept { + // Si NavCelId ya es entero/struct con hash, usa ese. Si no, adapta esto. + return std::hash{}(static_cast(c)); + } + }; + std::unordered_map<::navmap::NavCelId, float, NavCelIdHash> cell_max; + cell_max.reserve(fused.size()); // heurística + + size_t sidx = 0; // deja que locate lo actualice + std::optional<::navmap::NavCelId> last_cid; // hint + ::navmap::NavMap::LocateOpts opts; // reutilizado + Eigen::Vector3f bary; // reutilizado + Eigen::Vector3f hit; // reutilizado + + // Cota inferior rápida (si procede). Ajusta a tu nivel de suelo esperado. + constexpr float kMinAboveGround = 0.0f; + constexpr float kWriteThreshold = 0.1f; - auto t3 = parent_node_->now(); + auto t3 = parent_node_->now(); for (const auto & p : fused) { - if (std::isnan(p.x) || std::isinf(p.x)) {continue;} - + if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; + // Descarte barato: si ya sabes que p.z está por debajo del suelo esperado, sáltalo. + if (p.z < kMinAboveGround) continue; + ::navmap::NavCelId cid; - Eigen::Vector3f bary, hit; bool located = false; - { - ::navmap::NavMap::LocateOpts opts; - if (last_cid) opts.hint_cid = *last_cid; + // intento con hint + if (last_cid) { + opts.hint_cid = *last_cid; + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); + } else { + // evita basura en opts.hint_cid si el API la lee sin comprobar + opts = ::navmap::NavMap::LocateOpts{}; located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); } if (!located) { - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit); + // fallback sin hint + opts = ::navmap::NavMap::LocateOpts{}; + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); if (!located) continue; } last_cid = cid; const float h = static_cast(p.z) - hit.z(); - if ((h < 0.0f) || !std::isfinite(h)) continue; + if (!(h > kWriteThreshold)) continue; // incluye NaN/inf y <= threshold - if (h > 0.1) { - navmap_.layer_set(get_layer_name(), cid, h); + auto it = cell_max.find(cid); + if (it == cell_max.end()) { + cell_max.emplace(cid, h); + } else if (h > it->second) { + it->second = h; } } + // Aplicar a la capa en bloque + for (const auto & kv : cell_max) { + navmap_.layer_set(get_layer_name(), kv.first, kv.second); + } + auto t4 = parent_node_->now(); nav_state.set("map.navmap", navmap_); - auto t5 = parent_node_->now(); std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; @@ -119,6 +174,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } + } // namespace navmap } // namespace easynav #include From f1ebc8d9640af3e4c21ba167d5829c420704d534 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Tue, 11 Nov 2025 00:38:11 +0100 Subject: [PATCH 012/136] MPC was added --- .../easynav_mpc_controller/CMakeLists.txt | 94 ++++++ controllers/easynav_mpc_controller/README.md | 70 +++++ .../easynav_mpc_controller_plugins.xml | 9 + .../easynav_mpc_controller/MPCController.hpp | 82 +++++ .../easynav_mpc_controller/MPCOptimizer.hpp | 136 ++++++++ .../easynav_mpc_controller/package.xml | 31 ++ .../easynav_mpc_controller/MPCController.cpp | 160 ++++++++++ .../easynav_mpc_controller/MPCOptimizer.cpp | 296 ++++++++++++++++++ 8 files changed, 878 insertions(+) create mode 100644 controllers/easynav_mpc_controller/CMakeLists.txt create mode 100644 controllers/easynav_mpc_controller/README.md create mode 100644 controllers/easynav_mpc_controller/easynav_mpc_controller_plugins.xml create mode 100644 controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp create mode 100644 controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp create mode 100644 controllers/easynav_mpc_controller/package.xml create mode 100644 controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp create mode 100644 controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt new file mode 100644 index 00000000..74ed5902 --- /dev/null +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -0,0 +1,94 @@ +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() + +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(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) +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 +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${PCL_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + tf2_ros::tf2_ros + pluginlib::pluginlib + Eigen3::Eigen + nlopt + ${geometry_msgs_TARGETS} + ${nav_msgs_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() + + 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..655e8568 --- /dev/null +++ b/controllers/easynav_mpc_controller/README.md @@ -0,0 +1,70 @@ +# easynav_mppi_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)](#) [![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 | +|---|---| +| 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` +- **Library:** `easynav_mppi_controller` +- **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/...`. + +| Name | Type | Default | Description | +|---|---|---:|---| +| `.num_samples` | `int` | `100` | Number of trajectory rollouts per iteration. | +| `.horizon_steps` | `int` | `10` | Number of time steps in the prediction horizon. | +| `.dt` | `double` | `0.1` | Integration time step (seconds). | +| `.lambda` | `double` | `0.1` | Temperature / control noise scaling factor. | +| `.max_linear_velocity` | `double` | `1.0` | Maximum linear velocity (m/s). | +| `.max_angular_velocity` | `double` | `1.0` | Maximum angular velocity (rad/s). | +| `.max_linear_acceleration` | `double` | `0.5` | Maximum linear acceleration (m/s²). | +| `.max_angular_acceleration` | `double` | `1.0` | Maximum angular acceleration (rad/s²). | +| `.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. + + +## 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. | +| `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_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..5045c0ef --- /dev/null +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -0,0 +1,82 @@ +// 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. + +#ifndef EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ +#define EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#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 "easynav_core/ControllerMethodBase.hpp" +#include "easynav_common/types/NavState.hpp" + +namespace easynav +{ + +/// \brief A MPC Controller. +class MPCController : public ControllerMethodBase +{ +public: + MPCController(); + + /// \brief Destructor. + ~MPCController() override; + + /// \brief Initializes parameters and MPC controller. + /// \return std::expected success or error message. + std::expected 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 Predict the kinematic model for the robot. + /// \param x Current state, x, y and theta angle. + /// \param v Linear velocity command. + /// \param w Angular velocity command. + Eigen::Vector3d kinematic_model(const Eigen::Vector3d &x, double v, double w); + + /// \brief Cost function to optimize + /// \param u + /// \param grad + /// \param data + double cost_function(const std::vector &u, std::vector &grad, void *data); + + struct MPCParameters{ + Eigen::Vector2d goal; + Eigen::Vector3d x0; + }; + + struct NLoptCallbackData { + MPCController *controller; + void *user_data; + }; + +protected: + int horizon_steps_{5}; ///< Prediction horizon for MPC. + double dt_{0.1}; ///< Time step for MPC. + double max_lin_vel_{1.5}; ///< Maximum linear velocity for MPC. + double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. + +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..ba9ba858 --- /dev/null +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -0,0 +1,136 @@ +// 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_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ +#define EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ + +#include +#include +#include + +#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 Result structure for MPPI optimization containing control commands and trajectories. +struct MPPIResult +{ + double v; ///< Linear velocity command. + double w; ///< Angular velocity command. + std::vector>> all_trajectories; ///< All sampled trajectories. + std::vector> best_trajectory; ///< Best trajectory found during optimization. +}; + +struct TrajectorySample +{ + double v; ///< Linear velocity for this sample. + double w; ///< Angular velocity for this sample. + double cost; ///< Cost associated with this trajectory sample. +}; + +class MPPIOptimizer +{ +public: + /// \brief Constructor for MPPIOptimizer. + /// \param num_samples Number of samples to generate for MPPI. + /// \param horizon_steps Number of steps in the prediction horizon. + /// \param dt Time step for the simulation. + /// \param lambda Temperature parameter for MPPI. + /// \param max_lin_vel Maximum linear velocity in m/s. + /// \param max_ang_vel Maximum angular velocity in rad/s. + /// \param max_lin_acc Maximum linear acceleration in m/s^2. + /// \param max_ang_acc Maximum angular acceleration in rad/s^2. + /// \param fov Field of view in radians for trajectory sampling. + /// \param safety_radius Safety radius for obstacle avoidance in meters. + MPPIOptimizer( + double num_samples, double horizon_steps, double dt, double lambda, + double max_lin_vel = 1.0, double max_ang_vel = 1.0, double max_lin_acc = 1.0, + double max_ang_acc = 1.0, double fov = M_PI / 2.0, double safety_radius = 0.6); + + /// \brief Computes the control commands using MPPI optimization. + /// \param current_pose Current pose of the robot. + /// \param path Planned path to follow. + /// \param points Point cloud of the environment. + /// \return MPPIResult containing the best control commands and trajectories. + MPPIResult compute_control( + const geometry_msgs::msg::Pose & current_pose, + const nav_msgs::msg::Path & path, + const pcl::PointCloud & points); + +private: + double num_samples_; ///< Number of samples to generate for MPPI. + double horizon_steps_; ///< Number of steps in the prediction horizon. + double dt_; ///< Time step for the simulation. + double lambda_; ///< Temperature parameter for MPPI. + double max_lin_vel_; ///< Maximum linear velocity in m/s. + double max_ang_vel_; ///< Maximum angular velocity in rad/s. + double max_lin_acc_; ///< Maximum linear acceleration in m/s^2. + double max_ang_acc_; ///< Maximum angular acceleration in rad/s^2. + double fov_; ///< Field of view in radians for trajectory sampling. + double safety_radius_; ///< Safety radius for obstacle avoidance in meters. + double last_v_ = 0.0; ///< Last linear velocity command for smoothing. + double last_w_ = 0.0; ///< Last angular velocity command for smoothing. + + std::default_random_engine rng_; ///< Random number generator for sampling. + std::normal_distribution normal_ = std::normal_distribution(0.0, 0.5); ///< Normal distribution for noise in sampling. + std::normal_distribution v_noise_ = std::normal_distribution(0.0, 0.05); ///< Normal distribution for noise in linear velocity. + std::normal_distribution w_noise_ = std::normal_distribution(0.0, 0.02); ///< Normal distribution for noise in angular velocity. + + /// \brief Computes the cost of a trajectory based on its distance to the path and heading error. + /// \param trajectory The trajectory to evaluate. + /// \param path The planned path to follow. + /// \param v Linear velocity of the trajectory. + /// \param w Angular velocity of the trajectory. + /// \param initial_yaw Initial yaw orientation of the robot. + /// \param points The point cloud of the environment. + /// \return The computed cost of the trajectory. + double compute_cost( + const std::vector> & trajectory, + const nav_msgs::msg::Path & path, + double v, double w, double initial_yaw, + const pcl::PointCloud & points); + + /// \brief Simulates a trajectory based on initial position, orientation, and velocities. + /// \param x Initial x position. + /// \param y Initial y position. + /// \param yaw Initial yaw orientation. + /// \param v Linear velocity. + /// \param w Angular velocity. + /// \param path The planned path to follow. + /// \param steps Number of steps to simulate. + /// \return The simulated trajectory. + std::vector> simulate_trajectory( + double x, double y, double yaw, + double v, double w, const nav_msgs::msg::Path & path, int steps); + + /// \brief Computes the heading error between the robot's current orientation and the target point. + /// \param robot_yaw Current yaw orientation of the robot. + /// \param target_x X coordinate of the target point. + /// \param target_y Y coordinate of the target point. + /// \param robot_x X coordinate of the robot's current position. + /// \param robot_y Y coordinate of the robot's current position. + /// \return The heading error in radians. + double heading_error( + double robot_yaw, + double target_x, double target_y, + double robot_x, double robot_y); + + /// \brief Computes the shortest angular distance between two angles. + /// \param from Starting angle in radians. + /// \param to Ending angle in radians. + /// \return The shortest angular distance in radians. + double shortest_angular_distance(double from, double to); + +}; + +} // namespace easynav + +#endif // EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml new file mode 100644 index 00000000..3895e84e --- /dev/null +++ b/controllers/easynav_mpc_controller/package.xml @@ -0,0 +1,31 @@ + + + + easynav_mpc_controller + 0.0.1 + Easy Navigation: MPC Controller package. + Juan S. Cely G. + GPL-3.0-only + + ament_cmake + + easynav_common + easynav_core + pluginlib + tf2_ros + geometry_msgs + nav_msgs + visualization_msgs + pcl_ros + + 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..1cee1e3d --- /dev/null +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -0,0 +1,160 @@ +// 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" + +namespace easynav +{ + +MPCController::MPCController() {} + +MPCController::~MPCController() = default; + +std::expected +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 + ".max_linear_velocity", max_lin_vel_); + node->declare_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + + node->get_parameter(plugin_name + ".horizon_steps", horizon_steps_); + node->get_parameter(plugin_name + ".dt", dt_); + node->get_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); + node->get_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + + + + return {}; +} + + +Eigen::Vector3d +MPCController::kinematic_model(const Eigen::Vector3d &x, double v, double w) +{ + Eigen::Vector3d x_k1; + x_k1(0) = x(0) + v * cos(x(2)) * dt_; + x_k1(1) = x(1) + v * sin(x(2)) * dt_; + x_k1(2) = x(2) + w * dt_; + return x_k1; +} + + +double +MPCController::cost_function(const std::vector &u, std::vector &grad, void *data) +{ + MPCParameters *params = reinterpret_cast(data); + Eigen::Vector3d x = params->x0; + double cost = 0.0; + + for (int i = 0; i < horizon_steps_; ++i) { + double v = u[2*i]; + double w = u[2*i + 1]; + + x = kinematic_model(x, v, w); + Eigen::Vector2d pos = x.head<2>(); + Eigen::Vector2d error = pos - params->goal; + cost += error.squaredNorm() + 0.1 * (v*v + w*w); // ToDo quadratic function + } + + return cost; + +} + + +void +MPCController::update_rt(NavState & nav_state) +{ + if (!nav_state.has("path") || !nav_state.has("robot_pose")) { + return; + } + + const auto 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; + } + + int num_elements = path.poses.size(); + + 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 + + MPCParameters params; + params.x0 = {pose.position.x, pose.position.y, yaw_}; + const auto &last_pose = path.poses[num_elements - 1].pose.position; + params.goal = Eigen::Vector2d(static_cast(last_pose.x), + static_cast(last_pose.y)); + double minf; + + nlopt::opt opt(nlopt::LD_SLSQP, 2*horizon_steps_); + opt.set_min_objective(cost_function, ¶ms); + + std::vector lb(2*horizon_steps_, -max_lin_vel_); + std::vector ub(2*horizon_steps_, max_lin_vel_); + opt.set_lower_bounds(lb); + opt.set_upper_bounds(ub); + opt.set_xtol_rel(1e-4); + + std::vector u(2*horizon_steps_, 0.0); + + nlopt::result result = opt.optimize(u, minf); + + // try { + // nlopt::result result = opt.optimize(u, minf); + // } catch (std::exception &e) { + // std::cerr << "Optimization Error: " << e.what() << std::endl; + // break; + // } + + // 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_); +} + +} // 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..f0293d6f --- /dev/null +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -0,0 +1,296 @@ +#include "easynav_mppi_controller/MPCOptimizer.hpp" +#include "tf2/utils.hpp" + +#include +#include +#include +#include + +#include "easynav_common/types/Perceptions.hpp" +#include "easynav_common/types/PointPerception.hpp" + +namespace easynav +{ + +MPPIOptimizer::MPPIOptimizer( + double num_samples, double horizon_steps, double dt, double lambda, + double max_lin_vel, double max_ang_vel, double max_lin_acc, double max_ang_acc, + double fov, double safety_radius) +: num_samples_(num_samples), horizon_steps_(horizon_steps), dt_(dt), lambda_(lambda), + max_lin_vel_(max_lin_vel), max_ang_vel_(max_ang_vel), max_lin_acc_(max_lin_acc), + max_ang_acc_(max_ang_acc), + fov_(fov), safety_radius_(safety_radius) +{ +} + +std::vector> +MPPIOptimizer::simulate_trajectory( + double x, double y, double yaw, double v, double w, + const nav_msgs::msg::Path & path, int steps) +{ + size_t horizon_steps = static_cast(steps); + std::vector> trajectory; + trajectory.reserve(horizon_steps); + + bool has_path = !path.poses.empty(); + + for (size_t i = 0; i < horizon_steps; ++i) { + // MPPI noise sampling + double v_sample = v + normal_(rng_); + double w_sample = w + normal_(rng_); + + // Differential drive kinematics + x += v_sample * std::cos(yaw) * dt_; + y += v_sample * std::sin(yaw) * dt_; + yaw += w_sample * dt_; + + if (has_path) { + // Calculate the index in the path based on the current step + size_t path_idx = std::min(static_cast((i * path.poses.size()) / horizon_steps), + path.poses.size() - 1); + double target_x = path.poses[path_idx].pose.position.x; + double target_y = path.poses[path_idx].pose.position.y; + + // Smooth motion towards the target + double alpha = 0.2; + x += alpha * (target_x - x); + y += alpha * (target_y - y); + } + + trajectory.emplace_back(x, y); + } + + return trajectory; +} + +double MPPIOptimizer::heading_error( + double robot_yaw, + double target_x, double target_y, + double robot_x, double robot_y) +{ + double target_yaw = std::atan2(target_y - robot_y, target_x - robot_x); + double err = std::atan2(std::sin(target_yaw - robot_yaw), std::cos(target_yaw - robot_yaw)); + return std::abs(err); +} + +double MPPIOptimizer::shortest_angular_distance(double from, double to) +{ + double result = std::fmod(to - from + M_PI, 2.0 * M_PI); + if (result < 0) {result += 2.0 * M_PI;} + return result - M_PI; +} + +double MPPIOptimizer::compute_cost( + const std::vector> & trajectory, + const nav_msgs::msg::Path & path, + double v, double w, double initial_yaw, + const pcl::PointCloud & points) +{ + // Total cost accumulator + double cost = 0.0; + size_t path_size = path.poses.size(); + + // --- Path-Tracking Penalties --- + for (size_t i = 0; i < trajectory.size(); ++i) { + const auto &[x, y] = trajectory[i]; + + // Cost is distributed along the trajectory + double path_alpha = static_cast(i) / trajectory.size(); + size_t idx = std::min(static_cast(path_alpha * path_size), path_size - 1); + + double dx = path.poses[idx].pose.position.x - x; + double dy = path.poses[idx].pose.position.y - y; + double dist = std::hypot(dx, dy); + + // Heading error is calculated based on the initial yaw + double heading_penalty = heading_error(initial_yaw, path.poses[idx].pose.position.x, + path.poses[idx].pose.position.y, x, y); + + // FOV penalty: discourage trajectories outside robot's view + double angle_to_goal = heading_error(initial_yaw, trajectory.back().first, + trajectory.back().second, x, y); + double fov_penalty = std::pow(std::max(0.0, angle_to_goal - fov_), 2); + + // Accumulate penalties + cost += dist; // distance to path + cost += 0.05 * heading_penalty; // heading misalignment + cost += 0.2 * fov_penalty; // leaving field of view + } + + // --- Goal Progress Penalties --- + const auto & goal_pose = path.poses.back().pose; + const double gx = goal_pose.position.x; + const double gy = goal_pose.position.y; + + const auto & start_xy = trajectory.front(); + const auto & end_xy = trajectory.back(); + + double d_start_goal = std::hypot(gx - start_xy.first, gy - start_xy.second); + double d_end_goal = std::hypot(gx - end_xy.first, gy - end_xy.second); + + // how much closer we got to goal + double progress = d_start_goal - d_end_goal; + + cost += -2.0 * progress; // reward moving closer to goal + cost += 1.5 * d_end_goal; // penalize being far from goal at end + + // --- Obstacle Avoidance Penalties --- + double min_obs_overall = std::numeric_limits::max(); + + for (const auto & point : points) { + double min_obs_dist = std::numeric_limits::max(); + for (const auto &[x, y] : trajectory) { + double dx = point.x - x; + double dy = point.y - y; + double dist = std::hypot(dx, dy); + if (dist < min_obs_dist) {min_obs_dist = dist;} + } + min_obs_overall = std::min(min_obs_overall, min_obs_dist); + + // Safety margin (robot radius + margin) + if (min_obs_dist < safety_radius_) { + // Heavy penalty for collision risk + cost += 5000.0 * std::pow(safety_radius_ - min_obs_dist, 2) * (1.0 + v); + } else { + // Small penalty: encourage keeping clearance + cost += 1.0 / (min_obs_dist * min_obs_dist); + } + } + + // --- Velocity Encouragement --- + // If obstacles are far, discourage staying too slow + if (min_obs_overall > 0.6) { + cost += 0.2 / std::max(0.05, v); + } + + // Encourage smooth motions + double delta_v = v - last_v_; + double delta_w = w - last_w_; + cost += 0.1 * (delta_v * delta_v) + 0.1 * (delta_w * delta_w); + + // --- Regularization --- + // Smooth motions: penalize high linear/angular velocities + cost += 0.002 * (v * v) + 0.005 * (w * w); + + return cost; +} + +MPPIResult MPPIOptimizer::compute_control( + const geometry_msgs::msg::Pose & current_pose, + const nav_msgs::msg::Path & path, + const pcl::PointCloud & points) +{ + // If the path is empty, stop the robot + if (path.poses.empty()) { + return MPPIResult{0.0, 0.0, {}, {}}; + } + + // Current robot state + double x = current_pose.position.x; + double y = current_pose.position.y; + double yaw = tf2::getYaw(current_pose.orientation); + + // Select goal pose (within horizon, or last path point) + int last_pose = std::min(static_cast(horizon_steps_), path.poses.size() - 1); + int sim_steps = std::max(1, last_pose); + const auto & path_pose = path.poses[last_pose].pose; + double px = path_pose.position.x; + double py = path_pose.position.y; + double dist_to_goal = std::hypot(px - x, py - y); + + // Compute heading error to the goal + double angle_to_goal = std::atan2(py - y, px - x); + double angle_error = shortest_angular_distance(yaw, angle_to_goal); + + // Initialize sampling + std::vector samples; + samples.reserve(static_cast(num_samples_)); + + std::vector>> all_trajs; + std::vector> best_traj; + + // Base velocity proportional to heading + double angle_mag = std::abs(angle_error); + + // Scale linear velocity based on angular error + double v_scale = 1.0; + if (angle_mag > M_PI / 4.0) { // more than 45° + v_scale = 0.2; // move slowly + } else if (angle_mag > M_PI / 8.0) { // more than 22.5° and less than 45° + v_scale = 0.5; // move moderately + } + + // Base velocities + // Scale linear velocity based on distance to goal: closer to goal, faster we go + double base_v = max_lin_vel_ * std::min(dist_to_goal, 1.0) * v_scale; + base_v = std::clamp(base_v, 0.0, max_lin_vel_); + + // Angular velocity is proportional to the heading error: turn faster if more misaligned + double w_scale = std::min(1.0, 2.0 * angle_mag / M_PI); + double base_w = std::clamp(w_scale * angle_error, -max_ang_vel_, max_ang_vel_); + + // Generate candidate trajectories + for (int i = 0; i < num_samples_; ++i) { + // Sample noise for velocities with Gaussian distribution and clamp + double v = std::clamp(base_v + v_noise_(rng_), -max_lin_vel_, max_lin_vel_); + double w = std::clamp(base_w + w_noise_(rng_), -max_ang_vel_, max_ang_vel_); + + // Simulate trajectory and compute its cost + auto traj = simulate_trajectory(x, y, yaw, v, w, path, sim_steps); + double cost = compute_cost(traj, path, v, w, yaw, points); + all_trajs.push_back(traj); + + // Store the sample with its cost + samples.push_back({v, w, cost}); + } + + // Softmin: Find minimum cost among samples + auto best_sample_it = std::min_element(samples.begin(), samples.end(), + [](const auto & a, const auto & b) {return a.cost < b.cost;}); + + // Best trajectory and cost + best_traj = all_trajs[std::distance(samples.begin(), best_sample_it)]; + double min_cost = best_sample_it->cost; + + // Adapt lambda (temperature) if velocities collapse to near zero + double min_v_sample = std::numeric_limits::max(); + for (const auto & s : samples) { + min_v_sample = std::min(min_v_sample, s.v); + } + + if (min_v_sample < 0.05) { + lambda_ = std::min(5.0, lambda_ * 1.2); // increase lambda if tends to zero (stop) + } + + // Softmin weighting of samples + double denom = 0.0; + for (auto & sample : samples) { + sample.cost = std::exp(-1.0 / lambda_ * (sample.cost - min_cost)); + denom += sample.cost; + } + + // Weighted average of velocities + double vlin = 0.0, vrot = 0.0; + for (const auto & sample : samples) { + vlin += sample.v * sample.cost / denom; + vrot += sample.w * sample.cost / denom; + } + + // Calculate the maximum change in velocities based on acceleration limits + double max_v_delta = max_lin_acc_ * dt_; + double max_w_delta = max_ang_acc_ * dt_; + + // Limit velocity changes (slew rate) + vlin = last_v_ + std::clamp(vlin - last_v_, -max_v_delta, max_v_delta); + vrot = last_w_ + std::clamp(vrot - last_w_, -max_w_delta, max_w_delta); + + // Smooth velocities + double alpha_smooth = 0.2; // lower is more smooth + last_v_ = alpha_smooth * vlin + (1.0 - alpha_smooth) * last_v_; + last_w_ = alpha_smooth * vrot + (1.0 - alpha_smooth) * last_w_; + + // Return final control command and trajectories + return MPPIResult{last_v_, last_w_, all_trajs, best_traj}; +} + +} // namespace easynav From 92194d05b2248d9a984c057eae9ea5ce4d39b2ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:13:40 +0100 Subject: [PATCH 013/136] Add obstacle layer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../NavMapMapsManager.cpp | 9 +- .../filters/ObstacleFilter.cpp | 189 +++++++++--------- 2 files changed, 102 insertions(+), 96 deletions(-) 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 a2e3f54f..3ebc978a 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 @@ -25,7 +25,6 @@ #include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" -#include "easynav_common/plugin_diag.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -61,8 +60,6 @@ NavMapMapsManager::NavMapMapsManager() navmap_filters_loader_ = std::make_unique>( "easynav_navmap_maps_manager", "easynav::navmap::NavMapFilter"); - - ::easynav::dump_pluginlib_diagnostics(*navmap_filters_loader_, std::cerr, /*try_load_each_class=*/false); } NavMapMapsManager::~NavMapMapsManager() {} @@ -231,10 +228,10 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) filter->set_map_resolution(resolution_); filter->update(nav_state); - std::cerr << + std::cerr << "plugin name: " << filter->get_plugin_name() << "\tlayer name: " << filter->get_layer_name() << - "\tis_adding" << (filter->is_adding_layer()? "True" : "False") << std::endl; + "\tis_adding" << (filter->is_adding_layer() ? "True" : "False") << std::endl; navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); @@ -243,7 +240,7 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) 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/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index 3d7be5d6..a1855bbe 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 @@ -20,6 +20,7 @@ #include #include +#include #include "easynav_common/types/NavState.hpp" #include "easynav_common/types/Perceptions.hpp" @@ -51,127 +52,135 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) auto t0 = parent_node_->now(); std::cerr << "ObstacleFilter::update" << std::endl; - if (!nav_state.has("map.navmap")) 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"); - // Si necesitas limpieza total en cada ciclo, mantén esto: - navmap_.layer_clear(get_layer_name(), 0.0f); + navmap_.layer_clear(get_layer_name(), 0); auto t1 = parent_node_->now(); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(0.3) - .fuse(get_tf_prefix() + "map") - ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) - .as_points(); - - // (Opcional pero muy rentable) Orden espacial para mejorar locality del hint: - // ordenar por una clave Morton simple 2D (x,y). Implementación ligera: - auto morton2D = [](float x, float y) -> uint64_t { - // normaliza a rejilla 1 cm en [-64,64] m => 12800 pasos (cambia a tu rango) - auto q = [](float v){ - int32_t iv = static_cast(std::lrintf((v + 64.f) * 100.f)); - if (iv < 0) iv = 0; else if (iv > 12800) iv = 12800; - return static_cast(iv); - }; - auto part = [](uint32_t v){ - uint64_t x = v; - x = (x | (x << 16)) & 0x0000FFFF0000FFFFULL; - x = (x | (x << 8 )) & 0x00FF00FF00FF00FFULL; - x = (x | (x << 4 )) & 0x0F0F0F0F0F0F0F0FULL; - x = (x | (x << 2 )) & 0x3333333333333333ULL; - x = (x | (x << 1 )) & 0x5555555555555555ULL; - return x; - }; - uint64_t xi = part(q(x)); - uint64_t yi = part(q(y)) << 1; - return xi | yi; - }; - - std::sort(fused.begin(), fused.end(), - [&](const auto &a, const auto &b){ - return morton2D(a.x, a.y) < morton2D(b.x, b.y); - }); + const auto & points = PointPerceptionsOpsView(perceptions) + .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) + .downsample(0.3) + .fuse("map") + ->as_points(); auto t2 = parent_node_->now(); - // Acumulación de h máximos por celda (reduce layer_set) - struct NavCelIdHash { - std::size_t operator()(const ::navmap::NavCelId &c) const noexcept { - // Si NavCelId ya es entero/struct con hash, usa ese. Si no, adapta esto. - return std::hash{}(static_cast(c)); + 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<::navmap::NavCelId, float, NavCelIdHash> cell_max; - cell_max.reserve(fused.size()); // heurística - size_t sidx = 0; // deja que locate lo actualice - std::optional<::navmap::NavCelId> last_cid; // hint - ::navmap::NavMap::LocateOpts opts; // reutilizado - Eigen::Vector3f bary; // reutilizado - Eigen::Vector3f hit; // reutilizado + 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)); - // Cota inferior rápida (si procede). Ajusta a tu nivel de suelo esperado. - constexpr float kMinAboveGround = 0.0f; - constexpr float kWriteThreshold = 0.1f; + 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;} + } auto t3 = parent_node_->now(); - for (const auto & p : fused) { - if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; - // Descarte barato: si ya sabes que p.z está por debajo del suelo esperado, sáltalo. - if (p.z < kMinAboveGround) continue; + std::optional last_surface; + std::optional<::navmap::NavCelId> last_cid; - ::navmap::NavCelId cid; - bool located = false; - - // intento con hint - if (last_cid) { - opts.hint_cid = *last_cid; - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); - } else { - // evita basura en opts.hint_cid si el API la lee sin comprobar - opts = ::navmap::NavMap::LocateOpts{}; - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); - } + 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 (!located) { - // fallback sin hint - opts = ::navmap::NavMap::LocateOpts{}; - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); - if (!located) continue; + // std::cerr << "[ObstacleFilter] voxel (" << cx << ", " << cy << ") " + // << "vertical_bins=" << acc.z_bins.size() + // << " z_range=[" << acc.min_z << ", " << acc.max_z + // << "] Δz=" << dz << std::endl; + + if (acc.z_bins.size() <= 2 && dz <= height_threshold) { + continue; } - last_cid = cid; + const float cz = acc.max_z; + Eigen::Vector3f query(cx, cy, cz); - const float h = static_cast(p.z) - hit.z(); - if (!(h > kWriteThreshold)) continue; // incluye NaN/inf y <= threshold + 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); + } + } - auto it = cell_max.find(cid); - if (it == cell_max.end()) { - cell_max.emplace(cid, h); - } else if (h > it->second) { - it->second = h; + if (ok) { + navmap_.layer_set(get_layer_name(), cid, static_cast(255)); + last_surface = surface_idx; + last_cid = cid; } } - // Aplicar a la capa en bloque - for (const auto & kv : cell_max) { - navmap_.layer_set(get_layer_name(), kv.first, kv.second); - } auto t4 = parent_node_->now(); nav_state.set("map.navmap", navmap_); auto t5 = parent_node_->now(); - std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; - std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; - std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; - std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; - std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; + // std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; + // std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; + // std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; + // std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; + // std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } From 6211d8002d98e40175f5554edfd3ee210c65ff82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:13:52 +0100 Subject: [PATCH 014/136] Remove traces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 10 ---------- 1 file changed, 10 deletions(-) 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..afd13218 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -800,16 +800,6 @@ void AMCLLocalizer::correct(NavState & nav_state) } 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 ------------------- From 10a512aaf8a7f9489a5f50a56103638240cef1ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:14:23 +0100 Subject: [PATCH 015/136] A Star uses obstacle layer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_planner/AStarPlanner.cpp | 43 +++++++++++-------- 1 file changed, 26 insertions(+), 17 deletions(-) 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..57399091 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -296,7 +296,7 @@ std::vector AStarPlanner::a_star_path( if (nm.navcels.empty()) {return {};} - // 1) Locate start & goal navcels (fallback to closest triangle) + // 1) Locate start and goal NavCels (fallback to closest triangle if necessary) size_t sidx_s = 0, sidx_g = 0; NavCelId cid_start = 0, cid_goal = 0; Eigen::Vector3f bary; Eigen::Vector3f hit; @@ -317,7 +317,7 @@ std::vector AStarPlanner::a_star_path( const size_t N = nm.navcels.size(); - // 3) Precompute centroids (2D) for consistent metric and heuristic + // 2) Precompute centroids (used for cost and heuristic) std::vector C(N); for (NavCelId c = 0; c < N; ++c) { const auto cc = nm.navcel_centroid(c); @@ -328,24 +328,36 @@ std::vector AStarPlanner::a_star_path( const auto d = C[a] - C[b]; return static_cast(d.norm()); }; - auto step_cost = [&](NavCelId from, NavCelId to) -> double { - const double dist = static_cast((C[from] - C[to]).norm()); - return dist; + return static_cast((C[from] - C[to]).norm()); + }; + + // 3) Read the "obstacles" layer once and cache occupancy values + std::vector occ(N, FREE_SPACE); + for (NavCelId c = 0; c < N; ++c) { + // If the cell has no stored value, assume FREE_SPACE (0) + occ[c] = nm.layer_get("obstacles", c, FREE_SPACE); + } + auto is_free = [&](NavCelId c) -> bool { + // Strict policy: only 0 (FREE_SPACE) is traversable; any other value blocks + return occ[c] == FREE_SPACE; }; - // 4) A* on triangle graph + // If either start or goal falls in a non-free NavCel, do not plan + if (!is_free(cid_start) || !is_free(cid_goal)) { + return {}; + } + + // 4) Standard A* search on the triangle graph, skipping occupied NavCels 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; 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(); @@ -353,16 +365,13 @@ std::vector AStarPlanner::a_star_path( 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 - // } - for (NavCelId v : nm.navcel_neighbors(u)) { const size_t vidx = static_cast(v); if (vidx >= N) {continue;} + // ---- Occupancy check: skip non-free neighbors ---- + if (!is_free(v)) {continue;} + const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} @@ -372,16 +381,16 @@ std::vector AStarPlanner::a_star_path( parent[v] = u; const double f = tentative + h(v, cid_goal); open.push(Node{v, f}); - in_open[v] = 1; } } } + // If no valid path was found, return an empty list 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]) { geometry_msgs::msg::Pose p; @@ -394,7 +403,7 @@ std::vector AStarPlanner::a_star_path( } std::reverse(path.begin(), path.end()); - // Ensure at least goal pose + // Guarantee at least the goal pose if (path.empty()) {path.push_back(goal);} return path; } From c0c64a6fe05cf74a85175205ed065f048261089b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:21:02 +0100 Subject: [PATCH 016/136] Remove traces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../NavMapMapsManager.cpp | 6 ------ .../filters/ObstacleFilter.cpp | 18 ------------------ 2 files changed, 24 deletions(-) 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 3ebc978a..409beaf0 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 @@ -228,15 +228,9 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) filter->set_map_resolution(resolution_); filter->update(nav_state); - std::cerr << - "plugin name: " << filter->get_plugin_name() << - "\tlayer name: " << filter->get_layer_name() << - "\tis_adding" << (filter->is_adding_layer() ? "True" : "False") << std::endl; - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); if (filter->is_adding_layer() && navmap_.has_layer(filter->get_layer_name())) { - std::cerr << "Publishin layer " << filter->get_layer_name() << std::endl; 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/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index a1855bbe..6cfa7d5e 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 @@ -49,9 +49,6 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { - auto t0 = parent_node_->now(); - std::cerr << "ObstacleFilter::update" << std::endl; - if (!nav_state.has("map.navmap")) {return;} if (!nav_state.has("points")) {return;} @@ -60,16 +57,12 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) navmap_.layer_clear(get_layer_name(), 0); - auto t1 = parent_node_->now(); - const auto & points = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) .downsample(0.3) .fuse("map") ->as_points(); - auto t2 = parent_node_->now(); - const float voxel_xy = 0.30f; const float voxel_z = 0.20f; @@ -115,8 +108,6 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) if (z < acc.min_z) {acc.min_z = z;} } - auto t3 = parent_node_->now(); - std::optional last_surface; std::optional<::navmap::NavCelId> last_cid; @@ -171,16 +162,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) } } - - auto t4 = parent_node_->now(); nav_state.set("map.navmap", navmap_); - auto t5 = parent_node_->now(); - - // std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; - // std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; - // std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; - // std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; - // std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } From 4d476fff164ced7595b106f026b88faccf10a6f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:24:02 +0100 Subject: [PATCH 017/136] Remove commented traces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_maps_manager/filters/ObstacleFilter.cpp | 5 ----- 1 file changed, 5 deletions(-) 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 6cfa7d5e..cebeaff5 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 @@ -121,11 +121,6 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const float cy = (static_cast(key.iy) + 0.5f) * voxel_xy; const float dz = acc.max_z - acc.min_z; - // std::cerr << "[ObstacleFilter] voxel (" << cx << ", " << cy << ") " - // << "vertical_bins=" << acc.z_bins.size() - // << " z_range=[" << acc.min_z << ", " << acc.max_z - // << "] Δz=" << dz << std::endl; - if (acc.z_bins.size() <= 2 && dz <= height_threshold) { continue; } From ffccd69fc3b4da0c5bec5f1a649b0dfaebe7dc9e Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Tue, 11 Nov 2025 11:53:16 +0100 Subject: [PATCH 018/136] MPC terminated --- .../easynav_mpc_controller/MPCController.hpp | 29 ++---- .../easynav_mpc_controller/MPCController.cpp | 90 ++++++++++--------- 2 files changed, 56 insertions(+), 63 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 5045c0ef..28b1035a 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -24,6 +24,13 @@ #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" +struct MPCParameters{ + Eigen::Vector2d goal; + Eigen::Vector3d x0; + int N; + double dt; +}; + namespace easynav { @@ -44,28 +51,6 @@ class MPCController : public ControllerMethodBase /// \param nav_state Current navigation state, including odometry and planned path. void update_rt(NavState & nav_state) override; - /// \brief Predict the kinematic model for the robot. - /// \param x Current state, x, y and theta angle. - /// \param v Linear velocity command. - /// \param w Angular velocity command. - Eigen::Vector3d kinematic_model(const Eigen::Vector3d &x, double v, double w); - - /// \brief Cost function to optimize - /// \param u - /// \param grad - /// \param data - double cost_function(const std::vector &u, std::vector &grad, void *data); - - struct MPCParameters{ - Eigen::Vector2d goal; - Eigen::Vector3d x0; - }; - - struct NLoptCallbackData { - MPCController *controller; - void *user_data; - }; - protected: int horizon_steps_{5}; ///< Prediction horizon for MPC. double dt_{0.1}; ///< Time step for MPC. diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 1cee1e3d..65591208 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -22,6 +22,43 @@ #include "easynav_mpc_controller/MPCController.hpp" +Eigen::Vector3d +kinematic_model(const Eigen::Vector3d &x, double v, double w, double dt) +{ + Eigen::Vector3d x_k1; + x_k1(0) = x(0) + v * cos(x(2)) * dt; + x_k1(1) = x(1) + v * sin(x(2)) * dt; + x_k1(2) = x(2) + w * dt; + return x_k1; +} + +double +cost_function(const std::vector &u, std::vector &grad, void *data) +{ + MPCParameters *params = reinterpret_cast(data); + Eigen::Vector3d x = params->x0; + int N = params->N; + double dt = params->dt; + double cost = 0.0; + if (!grad.empty()) { + grad[0] = 0.0; + grad[1] = 0.5 / sqrt(x[1]); + } + + for (int i = 0; i < N; ++i) { + double v = u[2*i]; + double w = u[2*i + 1]; + + x = kinematic_model(x, v, w, dt); + Eigen::Vector2d pos = x.head<2>(); + Eigen::Vector2d error = pos - params->goal; + cost += error.squaredNorm() + 0.1 * (v*v + w*w); // ToDo quadratic function + } + + return cost; + +} + namespace easynav { @@ -50,40 +87,6 @@ MPCController::on_initialize() return {}; } - -Eigen::Vector3d -MPCController::kinematic_model(const Eigen::Vector3d &x, double v, double w) -{ - Eigen::Vector3d x_k1; - x_k1(0) = x(0) + v * cos(x(2)) * dt_; - x_k1(1) = x(1) + v * sin(x(2)) * dt_; - x_k1(2) = x(2) + w * dt_; - return x_k1; -} - - -double -MPCController::cost_function(const std::vector &u, std::vector &grad, void *data) -{ - MPCParameters *params = reinterpret_cast(data); - Eigen::Vector3d x = params->x0; - double cost = 0.0; - - for (int i = 0; i < horizon_steps_; ++i) { - double v = u[2*i]; - double w = u[2*i + 1]; - - x = kinematic_model(x, v, w); - Eigen::Vector2d pos = x.head<2>(); - Eigen::Vector2d error = pos - params->goal; - cost += error.squaredNorm() + 0.1 * (v*v + w*w); // ToDo quadratic function - } - - return cost; - -} - - void MPCController::update_rt(NavState & nav_state) { @@ -123,6 +126,8 @@ MPCController::update_rt(NavState & nav_state) const auto &last_pose = path.poses[num_elements - 1].pose.position; params.goal = Eigen::Vector2d(static_cast(last_pose.x), static_cast(last_pose.y)); + params.N = horizon_steps_; + params.dt = dt_; double minf; nlopt::opt opt(nlopt::LD_SLSQP, 2*horizon_steps_); @@ -136,14 +141,17 @@ MPCController::update_rt(NavState & nav_state) std::vector u(2*horizon_steps_, 0.0); - nlopt::result result = opt.optimize(u, minf); + // nlopt::result result = opt.optimize(u, minf); - // try { - // nlopt::result result = opt.optimize(u, minf); - // } catch (std::exception &e) { - // std::cerr << "Optimization Error: " << e.what() << std::endl; - // break; - // } + try { + nlopt::result result = opt.optimize(u, minf); + if (result != nlopt::SUCCESS) + { + std::cerr << "Optimization Error: " << std::endl; + } + } catch (std::exception &e) { + std::cerr << "Optimization Error: " << e.what() << std::endl; + } // Publish the computed velocity command cmd_vel_.header.frame_id = path.header.frame_id; From 090165c5f70db3b4801760eaf5d60dd656ef6733 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Tue, 11 Nov 2025 12:34:21 +0100 Subject: [PATCH 019/136] Dependecies were added --- controllers/easynav_mpc_controller/README.md | 23 +++++++------------ .../easynav_mpc_controller/package.xml | 3 +-- 2 files changed, 9 insertions(+), 17 deletions(-) diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md index 655e8568..7f814cf6 100644 --- a/controllers/easynav_mpc_controller/README.md +++ b/controllers/easynav_mpc_controller/README.md @@ -1,14 +1,14 @@ -# easynav_mppi_controller +# easynav_mpc_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)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) ## Description -A Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. +A Model Predictive Controller (MPC) implementation for Easy Navigation. ## Authors and Maintainers - **Authors:** Intelligent Robotics Lab -- **Maintainers:** Jose Miguel Guerrero Hernandez +- **Maintainers:** Juan S. Cely G. ## Supported ROS 2 Distributions | Distribution | Status | @@ -18,27 +18,21 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | ## Plugin (pluginlib) -- **Plugin Name:** `easynav_mppi_controller/MPPIController` -- **Type:** `easynav::MPPIController` +- **Plugin Name:** `easynav_mpc_controller/MPCController` +- **Type:** `easynav::MPCController` - **Base Class:** `easynav::ControllerMethodBase` -- **Library:** `easynav_mppi_controller` -- **Description:** A Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. +- **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_mppi_controller/MPPIController/...`. +All parameters are declared under the plugin namespace, i.e., `//easynav_mpc_controller/MPCController/...`. | Name | Type | Default | Description | |---|---|---:|---| -| `.num_samples` | `int` | `100` | Number of trajectory rollouts per iteration. | | `.horizon_steps` | `int` | `10` | Number of time steps in the prediction horizon. | | `.dt` | `double` | `0.1` | Integration time step (seconds). | -| `.lambda` | `double` | `0.1` | Temperature / control noise scaling factor. | | `.max_linear_velocity` | `double` | `1.0` | Maximum linear velocity (m/s). | | `.max_angular_velocity` | `double` | `1.0` | Maximum angular velocity (rad/s). | -| `.max_linear_acceleration` | `double` | `0.5` | Maximum linear acceleration (m/s²). | -| `.max_angular_acceleration` | `double` | `1.0` | Maximum angular acceleration (rad/s²). | -| `.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) @@ -59,7 +53,6 @@ This package does not create service servers or clients. |---|---|---|---| | `path` | `nav_msgs::msg::Path` | **Read** | Target path to track. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose/state. | -| `points` | `PointPerceptions` | **Read** | Perception point cloud(s) used for costs. | | `cmd_vel` | `geometry_msgs::msg::TwistStamped` | **Read** | Last commanded velocity (if provided in state). | diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 3895e84e..85209052 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -15,8 +15,7 @@ tf2_ros geometry_msgs nav_msgs - visualization_msgs - pcl_ros + NLopt rclcpp_lifecycle easynav_simple_common From 57d43ff4ee5400dc6dbd625e318c817cc90d0f94 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:26:50 +0100 Subject: [PATCH 020/136] Update to Occ constants MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_maps_manager/filters/ObstacleFilter.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 cebeaff5..c8c236dc 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 @@ -27,6 +27,7 @@ #include "easynav_common/types/PointPerception.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/ObstacleFilter.hpp" @@ -55,7 +56,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const auto & perceptions = nav_state.get("points"); navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - navmap_.layer_clear(get_layer_name(), 0); + navmap_.layer_clear(get_layer_name(), navmap_ros::FREE_SPACE); const auto & points = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) @@ -151,7 +152,8 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) } if (ok) { - navmap_.layer_set(get_layer_name(), cid, static_cast(255)); + navmap_.layer_set( + get_layer_name(), cid, static_cast(navmap_ros::LETHAL_OBSTACLE)); last_surface = surface_idx; last_cid = cid; } From 6b4f5ee84d64dab22a77e18e3d27506acdfec6c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:27:17 +0100 Subject: [PATCH 021/136] First functional version of Inflation filter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/InflationFilter.cpp | 147 +++++++++++------- 1 file changed, 87 insertions(+), 60 deletions(-) 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 3c023944..e79104e1 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 @@ -47,6 +47,7 @@ #include "easynav_common/types/PointPerception.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/InflationFilter.hpp" @@ -56,17 +57,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, @@ -76,94 +71,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;} @@ -183,44 +202,52 @@ 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_); + "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f", + inflation_radius_, cost_scaling_factor_, inscribed_radius_); return {}; } -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"); + auto nm = nav_state.get<::navmap::NavMap>("map.navmap"); + + const bool ok = inflate_layer_u8( + nm, + "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", nm); } - } // namespace navmap } // namespace easynav + #include PLUGINLIB_EXPORT_CLASS(easynav::navmap::InflationFilter, easynav::navmap::NavMapFilter) From 04c9e4a8194e59ceb60a5f1755c16a27bb5f965d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:27:42 +0100 Subject: [PATCH 022/136] Path planner aware of obstacles and inflation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_planner/AStarPlanner.cpp | 87 ++++++++++++++----- 1 file changed, 64 insertions(+), 23 deletions(-) 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 57399091..758c7277 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -35,18 +35,13 @@ #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; @@ -287,12 +282,19 @@ 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)); +} + 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 {};} @@ -317,47 +319,87 @@ std::vector AStarPlanner::a_star_path( const size_t N = nm.navcels.size(); - // 2) Precompute centroids (used for cost and heuristic) + // 2) Precompute centroids (used for cost, heuristic, and edge lengths) 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()}; } - auto h = [&](NavCelId a, NavCelId b) -> double { + auto euclid = [&](NavCelId a, NavCelId b) -> double { const auto d = C[a] - C[b]; return static_cast(d.norm()); }; - auto step_cost = [&](NavCelId from, NavCelId to) -> double { - return static_cast((C[from] - C[to]).norm()); - }; - // 3) Read the "obstacles" layer once and cache occupancy values + // 3) Choose cost layer: prefer "inflated_obstacles", fallback to "obstacles" + const std::string cost_layer = + layer_exists(nm, "inflated_obstacles") ? "inflated_obstacles" : "obstacles"; + + // Cache per-NavCel uint8_t cost values (0..255) std::vector occ(N, FREE_SPACE); for (NavCelId c = 0; c < N; ++c) { // If the cell has no stored value, assume FREE_SPACE (0) - occ[c] = nm.layer_get("obstacles", c, FREE_SPACE); + occ[c] = nm.layer_get(cost_layer, c, FREE_SPACE); } - auto is_free = [&](NavCelId c) -> bool { - // Strict policy: only 0 (FREE_SPACE) is traversable; any other value blocks - return occ[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); }; - // If either start or goal falls in a non-free NavCel, do not plan - if (!is_free(cid_start) || !is_free(cid_goal)) { + // 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 {}; } - // 4) Standard A* search on the triangle graph, skipping occupied NavCels + // 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 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* 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 parent(N, std::numeric_limits::max()); + std::vector in_open(N, 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(); @@ -369,8 +411,8 @@ std::vector AStarPlanner::a_star_path( const size_t vidx = static_cast(v); if (vidx >= N) {continue;} - // ---- Occupancy check: skip non-free neighbors ---- - if (!is_free(v)) {continue;} + // Skip non-traversable neighbors (lethal or unknown) + if (!traversable(v)) {continue;} const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} @@ -381,11 +423,11 @@ std::vector AStarPlanner::a_star_path( parent[v] = u; const double f = tentative + h(v, cid_goal); open.push(Node{v, f}); + in_open[v] = 1; } } } - // If no valid path was found, return an empty list if (!std::isfinite(g[cid_goal])) { return {}; } @@ -403,7 +445,6 @@ std::vector AStarPlanner::a_star_path( } std::reverse(path.begin(), path.end()); - // Guarantee at least the goal pose if (path.empty()) {path.push_back(goal);} return path; } From b10627859b061a5088cc4855d462e9a18c433042 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:27:54 +0100 Subject: [PATCH 023/136] Update sheets MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/README.md | 5 +- .../easynav_serest_controller/README.md | 3 +- .../easynav_simple_controller/README.md | 8 +- controllers/easynav_vff_controller/README.md | 5 +- .../easynav_costmap_localizer/README.md | 4 +- localizers/easynav_gps_localizer/README.md | 4 +- localizers/easynav_navmap_localizer/README.md | 4 +- localizers/easynav_simple_localizer/README.md | 4 +- .../easynav_bonxai_maps_manager/README.md | 4 +- .../easynav_costmap_maps_manager/README.md | 4 +- .../easynav_navmap_maps_manager/README.md | 98 +++++++++++++++---- .../easynav_simple_maps_manager/README.md | 4 +- planners/easynav_costmap_planner/README.md | 4 +- planners/easynav_navmap_planner/README.md | 57 +++++++---- planners/easynav_simple_planner/README.md | 4 +- 15 files changed, 158 insertions(+), 54 deletions(-) diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 655e8568..4daac078 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -1,6 +1,6 @@ # easynav_mppi_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)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-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 @@ -13,9 +13,10 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig ## 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` diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index f5d6fa49..0b2e200c 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -13,9 +13,10 @@ A SeReST (Smooth Error-Responsive Speed and Turning) controller for path trackin ## 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` diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index c01e2eb2..bf139bfc 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -1,6 +1,7 @@ # easynav_simple_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)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-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 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. @@ -12,9 +13,10 @@ Simple path-following controller that uses PID controllers and a look-ahead refe ## 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) | +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) - **Plugin Name:** `easynav_simple_controller/SimpleController` diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index ab2314ce..e6cd3315 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -1,6 +1,7 @@ # 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)](#) +[![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 Easy Navigation: VFF Controller package. @@ -12,6 +13,8 @@ Easy Navigation: VFF Controller package. ## 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/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8d3ae420..c2f8ce66 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for prediction. @@ -12,6 +12,8 @@ AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for predi ## 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/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index a989cfa3..b3b44698 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. @@ -12,6 +12,8 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- ## 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/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index 93ce6657..c2229864 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 Easy Navigation: nAVmAP Localizer package. @@ -12,6 +12,8 @@ Easy Navigation: nAVmAP Localizer package. ## 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/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 04505e89..2de8ce67 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 AMCL-style localizer using a simple 2D grid-like map for scoring and odometry for prediction. @@ -12,6 +12,8 @@ AMCL-style localizer using a simple 2D grid-like map for scoring and odometry fo ## 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/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index 4df2fba2..bb3c03c9 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 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. @@ -12,6 +12,8 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) ## 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/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index e3a54d13..195f710a 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 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. @@ -19,6 +19,8 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends ## 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/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 1f7ce673..2a45f1cb 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -1,66 +1,124 @@ # 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)](#) +[![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 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_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 413125aa..8c002e1e 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 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. @@ -14,6 +14,8 @@ At the heart of this stack is the SimpleMap data structure. It represents the en ## 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_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index fff2666f..dbea0d9e 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) | diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 58d80e1b..fa7170c6 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). | - +| `.layer` | `string` | `"inflated_obstacles"` | Name of the NavMap layer containing 8-bit costs (`FREE_SPACE=0`, `INSCRIBED_INFLATED_OBSTACLE=253`, `LETHAL_OBSTACLE=254`). | +| `.cost_factor` | `double` | `2.0` | Multiplicative weight for geometric distance; values > 1 increase the relative importance of distance. | +| `.inflation_penalty` | `double` | `5.0` | Additional penalty proportional to the normalized cell cost; higher values keep the path farther from inflated areas. | +| `.continuous_replan` | `bool` | `true` | If true, recomputes the path whenever `NavState` updates; if false, plans once per goal. | ## Interfaces (Topics and Services) @@ -40,20 +62,19 @@ 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_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) | From 1b7acafc4b632dc96ce43ebf8f69642ab6eff96f Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 12 Nov 2025 17:28:00 +0100 Subject: [PATCH 024/136] Optimizer was changed --- .../easynav_mpc_controller/MPCController.cpp | 67 +++++++++++++------ 1 file changed, 47 insertions(+), 20 deletions(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 65591208..59443931 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -26,9 +26,9 @@ Eigen::Vector3d kinematic_model(const Eigen::Vector3d &x, double v, double w, double dt) { Eigen::Vector3d x_k1; - x_k1(0) = x(0) + v * cos(x(2)) * dt; - x_k1(1) = x(1) + v * sin(x(2)) * dt; - x_k1(2) = x(2) + w * dt; + x_k1[0] = x[0] + v * cos(x[2]) * dt; + x_k1[1] = x[1] + v * sin(x[2]) * dt; + x_k1[2] = x[2] + w * dt; return x_k1; } @@ -41,24 +41,40 @@ cost_function(const std::vector &u, std::vector &grad, void *dat double dt = params->dt; double cost = 0.0; if (!grad.empty()) { - grad[0] = 0.0; - grad[1] = 0.5 / sqrt(x[1]); + grad.assign(u.size(), 0.0); } + std::cout << "====="<< std::endl; + std::cout << "u: "<< u[0] << std::endl; + std::cout << "N: "<< params->N << std::endl; + for (int i = 0; i < N; ++i) { double v = u[2*i]; double w = u[2*i + 1]; + // std::cout << "===== "<< std::endl; + // std::cout << "x: "<< x << std::endl; + // std::cout << "v: "<< v << std::endl; + // std::cout << "w: "<< w << std::endl; + // std::cout << "dt: "<< dt << std::endl; + x = kinematic_model(x, v, w, dt); Eigen::Vector2d pos = x.head<2>(); Eigen::Vector2d error = pos - params->goal; - cost += error.squaredNorm() + 0.1 * (v*v + w*w); // ToDo quadratic function + cost += /*error.squaredNorm() +*/ 0.1 * (v*v + w*w); // ToDo quadratic function } - + return cost; } +static double nlopt_cost_callback(const std::vector &x, + std::vector &grad, + void *data) +{ + return cost_function(x, grad, data); +} + namespace easynav { @@ -91,6 +107,7 @@ void MPCController::update_rt(NavState & nav_state) { if (!nav_state.has("path") || !nav_state.has("robot_pose")) { + std::cout << "No Path or No robot pose" << std::endl; return; } @@ -98,6 +115,7 @@ MPCController::update_rt(NavState & nav_state) if (path.poses.empty()) { // If the path is empty, stop the robot + std::cout << "Path Empty" << std::endl; cmd_vel_.header.frame_id = path.header.frame_id; cmd_vel_.header.stamp = get_node()->now(); cmd_vel_.twist.linear.x = 0.0; @@ -107,6 +125,7 @@ MPCController::update_rt(NavState & nav_state) } int num_elements = path.poses.size(); + std::cout << "num_elemnts" << std::endl; const auto pose = nav_state.get("robot_pose").pose.pose; double roll_, pitch_, yaw_; @@ -121,6 +140,8 @@ MPCController::update_rt(NavState & nav_state) // MPC Code + std::cout << "MPC elements" << std::endl; + MPCParameters params; params.x0 = {pose.position.x, pose.position.y, yaw_}; const auto &last_pose = path.poses[num_elements - 1].pose.position; @@ -129,9 +150,13 @@ MPCController::update_rt(NavState & nav_state) params.N = horizon_steps_; params.dt = dt_; double minf; + std::vector u(2*horizon_steps_, 0.0); - nlopt::opt opt(nlopt::LD_SLSQP, 2*horizon_steps_); - opt.set_min_objective(cost_function, ¶ms); + // nlopt::opt opt(nlopt::LD_SLSQP, static_cast(u.size())); + nlopt::opt opt(nlopt::LN_COBYLA, static_cast(u.size())); + opt.set_min_objective(nlopt_cost_callback, ¶ms); + + std::cout << "Set cost function" << std::endl; std::vector lb(2*horizon_steps_, -max_lin_vel_); std::vector ub(2*horizon_steps_, max_lin_vel_); @@ -139,19 +164,21 @@ MPCController::update_rt(NavState & nav_state) opt.set_upper_bounds(ub); opt.set_xtol_rel(1e-4); - std::vector u(2*horizon_steps_, 0.0); + std::cout << "Configurated optimizer" << std::endl; - // nlopt::result result = opt.optimize(u, minf); + nlopt::result result = opt.optimize(u, minf); - try { - nlopt::result result = opt.optimize(u, minf); - if (result != nlopt::SUCCESS) - { - std::cerr << "Optimization Error: " << std::endl; - } - } catch (std::exception &e) { - std::cerr << "Optimization Error: " << e.what() << std::endl; - } + // try { + // nlopt::result result = opt.optimize(u, minf); + // std::cout << "Optimizo" << std::endl; + // if (result != nlopt::SUCCESS) + // { + // std::cerr << "Optimization Unsuccessful " << std::endl; + // } + // } catch (std::exception &e) { + // std::cerr << "Optimization Error: " << e.what() << std::endl; + // } + std::cout << u[0] << ", " << u[1] << std::endl; // Publish the computed velocity command cmd_vel_.header.frame_id = path.header.frame_id; From ced2f38a69f463bfa6e667680f209a9706ce4f2b Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 13 Nov 2025 00:56:25 +0100 Subject: [PATCH 025/136] MPC controller is working --- .../easynav_mpc_controller/MPCController.hpp | 6 ++++ .../easynav_mpc_controller/MPCController.cpp | 33 ++++++++++++++++--- 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 28b1035a..384586d7 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -27,6 +27,9 @@ struct MPCParameters{ Eigen::Vector2d goal; Eigen::Vector3d x0; + Eigen::Matrix2d Q; + Eigen::Matrix2d R; + Eigen::Matrix2d Rd; int N; double dt; }; @@ -58,6 +61,9 @@ class MPCController : public ControllerMethodBase double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. private: + Eigen::Matrix2d Q_ {{1.0, 0.0},{0.0, 1.0}}; + Eigen::Matrix2d R_ {{0.01, 0.0},{0.0, 0.01}}; + Eigen::Matrix2d Rd_ {{0.01, 0.0},{0.0, 1.0}}; geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. }; diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 59443931..666a1474 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -36,21 +36,36 @@ double cost_function(const std::vector &u, std::vector &grad, void *data) { MPCParameters *params = reinterpret_cast(data); + Eigen::Vector3d x = params->x0; int N = params->N; double dt = params->dt; double cost = 0.0; + + Eigen::Matrix2d R = params->R; + Eigen::Matrix2d Q = params->Q; + Eigen::Matrix2d Rd = params->Rd; + if (!grad.empty()) { grad.assign(u.size(), 0.0); } - std::cout << "====="<< std::endl; - std::cout << "u: "<< u[0] << std::endl; - std::cout << "N: "<< params->N << std::endl; - + // std::cout << "====="<< std::endl; + // std::cout << "u: "<< u[0] << std::endl; + // std::cout << "N: "<< params->N << std::endl; + 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; + } // std::cout << "===== "<< std::endl; // std::cout << "x: "<< x << std::endl; @@ -59,9 +74,14 @@ cost_function(const std::vector &u, std::vector &grad, void *dat // std::cout << "dt: "<< dt << std::endl; x = kinematic_model(x, v, w, dt); + Eigen::Vector2d pos = x.head<2>(); Eigen::Vector2d error = pos - params->goal; - cost += /*error.squaredNorm() +*/ 0.1 * (v*v + w*w); // ToDo quadratic function + Eigen::Vector2d uk(v, w); + Eigen::Vector2d duk(dv, dw); + + // cost += (uk.transpose()*R)*uk + (error.transpose()*Q)*error + (duk.transpose()*Rd)*duk; + cost += uk.dot(R * uk) + error.dot(Q * error) + duk.dot(Rd * duk); } return cost; @@ -149,6 +169,9 @@ MPCController::update_rt(NavState & nav_state) static_cast(last_pose.y)); params.N = horizon_steps_; params.dt = dt_; + params.R = R_; + params.Q = Q_; + params.Rd = Rd_; double minf; std::vector u(2*horizon_steps_, 0.0); From b8df639665df68e60fa805ef4cef0c04f15a4502 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 13 Nov 2025 08:35:39 +0100 Subject: [PATCH 026/136] Update Readme MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- planners/easynav_navmap_planner/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index fa7170c6..164419dc 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -73,8 +73,7 @@ This plugin does not create subscriptions or services directly; it retrieves all | `path` | `nav_msgs::msg::Path` | **Write** | Output path, computed as the lowest-cost route. | ## TF Frames -The planner assumes frame consistency between NavMap, robot pose, and goals. -No TF lookups are performed internally. +The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally. ## License GPL-3.0-only From f87388ee69e0d1f407ae71748b239d21eb887734 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 10 Nov 2025 06:58:51 +0100 Subject: [PATCH 027/136] First functional verison, but expensive MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CMakeLists.txt | 4 +- .../filters/NavMapFilter.hpp | 5 +- .../filters/ObstacleFilter.hpp | 4 +- .../NavMapMapsManager.cpp | 56 +++++++++------- .../filters/ObstacleFilter.cpp | 67 ++++++++++++++----- 5 files changed, 85 insertions(+), 51 deletions(-) diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index 09006d4a..f1e05f04 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt @@ -90,8 +90,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/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..6733a9d0 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 @@ -53,11 +53,10 @@ 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; +protected: + std::shared_ptr get_node() const; const std::string & get_tf_prefix() const; protected: 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..38833490 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 @@ -44,8 +44,8 @@ class ObstacleFilter : public NavMapFilter virtual std::expected on_initialize(); virtual void update(::easynav::NavState & nav_state); - 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/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index 7725bcc9..a2e3f54f 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 @@ -25,6 +25,7 @@ #include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" +#include "easynav_common/plugin_diag.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -45,15 +46,23 @@ 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"); + + ::easynav::dump_pluginlib_diagnostics(*navmap_filters_loader_, std::cerr, /*try_load_each_class=*/false); } NavMapMapsManager::~NavMapMapsManager() {} @@ -85,7 +94,6 @@ 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); @@ -219,27 +227,23 @@ 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); -// } -// } + std::cerr << + "plugin name: " << filter->get_plugin_name() << + "\tlayer name: " << filter->get_layer_name() << + "\tis_adding" << (filter->is_adding_layer()? "True" : "False") << std::endl; + + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + + if (filter->is_adding_layer() && navmap_.has_layer(filter->get_layer_name())) { + std::cerr << "Publishin layer " << filter->get_layer_name() << std::endl; + 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/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index faec9835..af2c3293 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 @@ -49,7 +49,10 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { - if (!nav_state.has("map")) { + auto t0 = parent_node_->now(); + + std::cerr << "ObstacleFilter::update" << std::endl; + if (!nav_state.has("map.navmap")) { return; } if (!nav_state.has("points")) { @@ -57,35 +60,63 @@ ObstacleFilter::update(::easynav::NavState & nav_state) } const auto & perceptions = nav_state.get("points"); - navmap_ = nav_state.get<::navmap::NavMap>("map"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - if (!navmap_.layer_copy("occupancy", "obstacles")) { - RCLCPP_ERROR(parent_node_->get_logger(), "Error copying layers at ObstacleFilter"); - return; - } + navmap_.layer_clear(get_layer_name(), 0.0f); + + auto t1 = parent_node_->now(); auto fused = PointPerceptionsOpsView(perceptions) - .downsample(get_map_resolution()) + .downsample(0.1) .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) .as_points(); + auto t2 = parent_node_->now(); + size_t sidx = 0; + std::optional<::navmap::NavCelId> last_cid; + + auto t3 = parent_node_->now(); + for (const auto & p : fused) { - if (std::isnan(p.x)) {continue;} + if (std::isnan(p.x) || std::isinf(p.x)) {continue;} + ::navmap::NavCelId cid; - Eigen::Vector3f bary; - Eigen::Vector3f hit; - - 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); - } + Eigen::Vector3f bary, hit; + bool located = false; + + { + ::navmap::NavMap::LocateOpts opts; + if (last_cid) opts.hint_cid = *last_cid; + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); + } + + if (!located) { + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit); + if (!located) continue; + } + + last_cid = cid; + + const float h = static_cast(p.z) - hit.z(); + if ((h < 0.0f) || !std::isfinite(h)) continue; + + if (h > 0.1) { + navmap_.layer_set(get_layer_name(), cid, h); } } - nav_state.set("map", navmap_); + auto t4 = parent_node_->now(); + nav_state.set("map.navmap", navmap_); + + auto t5 = parent_node_->now(); + + std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; + std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; + std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; + std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; + std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } } // namespace navmap From fcb5c5615055e0c437330ce24a99ee28035c950f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 10 Nov 2025 07:12:57 +0100 Subject: [PATCH 028/136] Optimized function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/ObstacleFilter.cpp | 110 +++++++++++++----- 1 file changed, 83 insertions(+), 27 deletions(-) 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 af2c3293..3d7be5d6 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 @@ -46,70 +46,125 @@ ObstacleFilter::on_initialize() return {}; } -void -ObstacleFilter::update(::easynav::NavState & nav_state) +void ObstacleFilter::update(::easynav::NavState & nav_state) { auto t0 = parent_node_->now(); - std::cerr << "ObstacleFilter::update" << std::endl; - if (!nav_state.has("map.navmap")) { - 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"); + // Si necesitas limpieza total en cada ciclo, mantén esto: navmap_.layer_clear(get_layer_name(), 0.0f); auto t1 = parent_node_->now(); auto fused = PointPerceptionsOpsView(perceptions) - .downsample(0.1) - .fuse(get_tf_prefix() + "map") - ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) - .as_points(); + .downsample(0.3) + .fuse(get_tf_prefix() + "map") + ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) + .as_points(); + + // (Opcional pero muy rentable) Orden espacial para mejorar locality del hint: + // ordenar por una clave Morton simple 2D (x,y). Implementación ligera: + auto morton2D = [](float x, float y) -> uint64_t { + // normaliza a rejilla 1 cm en [-64,64] m => 12800 pasos (cambia a tu rango) + auto q = [](float v){ + int32_t iv = static_cast(std::lrintf((v + 64.f) * 100.f)); + if (iv < 0) iv = 0; else if (iv > 12800) iv = 12800; + return static_cast(iv); + }; + auto part = [](uint32_t v){ + uint64_t x = v; + x = (x | (x << 16)) & 0x0000FFFF0000FFFFULL; + x = (x | (x << 8 )) & 0x00FF00FF00FF00FFULL; + x = (x | (x << 4 )) & 0x0F0F0F0F0F0F0F0FULL; + x = (x | (x << 2 )) & 0x3333333333333333ULL; + x = (x | (x << 1 )) & 0x5555555555555555ULL; + return x; + }; + uint64_t xi = part(q(x)); + uint64_t yi = part(q(y)) << 1; + return xi | yi; + }; + + std::sort(fused.begin(), fused.end(), + [&](const auto &a, const auto &b){ + return morton2D(a.x, a.y) < morton2D(b.x, b.y); + }); auto t2 = parent_node_->now(); - size_t sidx = 0; - std::optional<::navmap::NavCelId> last_cid; + // Acumulación de h máximos por celda (reduce layer_set) + struct NavCelIdHash { + std::size_t operator()(const ::navmap::NavCelId &c) const noexcept { + // Si NavCelId ya es entero/struct con hash, usa ese. Si no, adapta esto. + return std::hash{}(static_cast(c)); + } + }; + std::unordered_map<::navmap::NavCelId, float, NavCelIdHash> cell_max; + cell_max.reserve(fused.size()); // heurística + + size_t sidx = 0; // deja que locate lo actualice + std::optional<::navmap::NavCelId> last_cid; // hint + ::navmap::NavMap::LocateOpts opts; // reutilizado + Eigen::Vector3f bary; // reutilizado + Eigen::Vector3f hit; // reutilizado + + // Cota inferior rápida (si procede). Ajusta a tu nivel de suelo esperado. + constexpr float kMinAboveGround = 0.0f; + constexpr float kWriteThreshold = 0.1f; - auto t3 = parent_node_->now(); + auto t3 = parent_node_->now(); for (const auto & p : fused) { - if (std::isnan(p.x) || std::isinf(p.x)) {continue;} - + if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; + // Descarte barato: si ya sabes que p.z está por debajo del suelo esperado, sáltalo. + if (p.z < kMinAboveGround) continue; + ::navmap::NavCelId cid; - Eigen::Vector3f bary, hit; bool located = false; - { - ::navmap::NavMap::LocateOpts opts; - if (last_cid) opts.hint_cid = *last_cid; + // intento con hint + if (last_cid) { + opts.hint_cid = *last_cid; + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); + } else { + // evita basura en opts.hint_cid si el API la lee sin comprobar + opts = ::navmap::NavMap::LocateOpts{}; located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); } if (!located) { - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit); + // fallback sin hint + opts = ::navmap::NavMap::LocateOpts{}; + located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); if (!located) continue; } last_cid = cid; const float h = static_cast(p.z) - hit.z(); - if ((h < 0.0f) || !std::isfinite(h)) continue; + if (!(h > kWriteThreshold)) continue; // incluye NaN/inf y <= threshold - if (h > 0.1) { - navmap_.layer_set(get_layer_name(), cid, h); + auto it = cell_max.find(cid); + if (it == cell_max.end()) { + cell_max.emplace(cid, h); + } else if (h > it->second) { + it->second = h; } } + // Aplicar a la capa en bloque + for (const auto & kv : cell_max) { + navmap_.layer_set(get_layer_name(), kv.first, kv.second); + } + auto t4 = parent_node_->now(); nav_state.set("map.navmap", navmap_); - auto t5 = parent_node_->now(); std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; @@ -119,6 +174,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } + } // namespace navmap } // namespace easynav #include From e4aaa008f4f44dbe74825e5f113adcc64a0cd225 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:13:40 +0100 Subject: [PATCH 029/136] Add obstacle layer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../NavMapMapsManager.cpp | 9 +- .../filters/ObstacleFilter.cpp | 189 +++++++++--------- 2 files changed, 102 insertions(+), 96 deletions(-) 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 a2e3f54f..3ebc978a 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 @@ -25,7 +25,6 @@ #include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_common/YTSession.hpp" -#include "easynav_common/plugin_diag.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -61,8 +60,6 @@ NavMapMapsManager::NavMapMapsManager() navmap_filters_loader_ = std::make_unique>( "easynav_navmap_maps_manager", "easynav::navmap::NavMapFilter"); - - ::easynav::dump_pluginlib_diagnostics(*navmap_filters_loader_, std::cerr, /*try_load_each_class=*/false); } NavMapMapsManager::~NavMapMapsManager() {} @@ -231,10 +228,10 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) filter->set_map_resolution(resolution_); filter->update(nav_state); - std::cerr << + std::cerr << "plugin name: " << filter->get_plugin_name() << "\tlayer name: " << filter->get_layer_name() << - "\tis_adding" << (filter->is_adding_layer()? "True" : "False") << std::endl; + "\tis_adding" << (filter->is_adding_layer() ? "True" : "False") << std::endl; navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); @@ -243,7 +240,7 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) 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/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index 3d7be5d6..a1855bbe 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 @@ -20,6 +20,7 @@ #include #include +#include #include "easynav_common/types/NavState.hpp" #include "easynav_common/types/Perceptions.hpp" @@ -51,127 +52,135 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) auto t0 = parent_node_->now(); std::cerr << "ObstacleFilter::update" << std::endl; - if (!nav_state.has("map.navmap")) 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"); - // Si necesitas limpieza total en cada ciclo, mantén esto: - navmap_.layer_clear(get_layer_name(), 0.0f); + navmap_.layer_clear(get_layer_name(), 0); auto t1 = parent_node_->now(); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(0.3) - .fuse(get_tf_prefix() + "map") - ->filter({-5.0, -5.0, NAN}, {5.0, 5.0, NAN}) - .as_points(); - - // (Opcional pero muy rentable) Orden espacial para mejorar locality del hint: - // ordenar por una clave Morton simple 2D (x,y). Implementación ligera: - auto morton2D = [](float x, float y) -> uint64_t { - // normaliza a rejilla 1 cm en [-64,64] m => 12800 pasos (cambia a tu rango) - auto q = [](float v){ - int32_t iv = static_cast(std::lrintf((v + 64.f) * 100.f)); - if (iv < 0) iv = 0; else if (iv > 12800) iv = 12800; - return static_cast(iv); - }; - auto part = [](uint32_t v){ - uint64_t x = v; - x = (x | (x << 16)) & 0x0000FFFF0000FFFFULL; - x = (x | (x << 8 )) & 0x00FF00FF00FF00FFULL; - x = (x | (x << 4 )) & 0x0F0F0F0F0F0F0F0FULL; - x = (x | (x << 2 )) & 0x3333333333333333ULL; - x = (x | (x << 1 )) & 0x5555555555555555ULL; - return x; - }; - uint64_t xi = part(q(x)); - uint64_t yi = part(q(y)) << 1; - return xi | yi; - }; - - std::sort(fused.begin(), fused.end(), - [&](const auto &a, const auto &b){ - return morton2D(a.x, a.y) < morton2D(b.x, b.y); - }); + const auto & points = PointPerceptionsOpsView(perceptions) + .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) + .downsample(0.3) + .fuse("map") + ->as_points(); auto t2 = parent_node_->now(); - // Acumulación de h máximos por celda (reduce layer_set) - struct NavCelIdHash { - std::size_t operator()(const ::navmap::NavCelId &c) const noexcept { - // Si NavCelId ya es entero/struct con hash, usa ese. Si no, adapta esto. - return std::hash{}(static_cast(c)); + 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<::navmap::NavCelId, float, NavCelIdHash> cell_max; - cell_max.reserve(fused.size()); // heurística - size_t sidx = 0; // deja que locate lo actualice - std::optional<::navmap::NavCelId> last_cid; // hint - ::navmap::NavMap::LocateOpts opts; // reutilizado - Eigen::Vector3f bary; // reutilizado - Eigen::Vector3f hit; // reutilizado + 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)); - // Cota inferior rápida (si procede). Ajusta a tu nivel de suelo esperado. - constexpr float kMinAboveGround = 0.0f; - constexpr float kWriteThreshold = 0.1f; + 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;} + } auto t3 = parent_node_->now(); - for (const auto & p : fused) { - if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; - // Descarte barato: si ya sabes que p.z está por debajo del suelo esperado, sáltalo. - if (p.z < kMinAboveGround) continue; + std::optional last_surface; + std::optional<::navmap::NavCelId> last_cid; - ::navmap::NavCelId cid; - bool located = false; - - // intento con hint - if (last_cid) { - opts.hint_cid = *last_cid; - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); - } else { - // evita basura en opts.hint_cid si el API la lee sin comprobar - opts = ::navmap::NavMap::LocateOpts{}; - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); - } + 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 (!located) { - // fallback sin hint - opts = ::navmap::NavMap::LocateOpts{}; - located = navmap_.locate_navcel({p.x, p.y, p.z}, sidx, cid, bary, &hit, opts); - if (!located) continue; + // std::cerr << "[ObstacleFilter] voxel (" << cx << ", " << cy << ") " + // << "vertical_bins=" << acc.z_bins.size() + // << " z_range=[" << acc.min_z << ", " << acc.max_z + // << "] Δz=" << dz << std::endl; + + if (acc.z_bins.size() <= 2 && dz <= height_threshold) { + continue; } - last_cid = cid; + const float cz = acc.max_z; + Eigen::Vector3f query(cx, cy, cz); - const float h = static_cast(p.z) - hit.z(); - if (!(h > kWriteThreshold)) continue; // incluye NaN/inf y <= threshold + 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); + } + } - auto it = cell_max.find(cid); - if (it == cell_max.end()) { - cell_max.emplace(cid, h); - } else if (h > it->second) { - it->second = h; + if (ok) { + navmap_.layer_set(get_layer_name(), cid, static_cast(255)); + last_surface = surface_idx; + last_cid = cid; } } - // Aplicar a la capa en bloque - for (const auto & kv : cell_max) { - navmap_.layer_set(get_layer_name(), kv.first, kv.second); - } auto t4 = parent_node_->now(); nav_state.set("map.navmap", navmap_); auto t5 = parent_node_->now(); - std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; - std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; - std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; - std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; - std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; + // std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; + // std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; + // std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; + // std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; + // std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } From eccc41a61e96fa164a9bd69de6d889beefacb744 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:13:52 +0100 Subject: [PATCH 030/136] Remove traces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 10 ---------- 1 file changed, 10 deletions(-) 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..afd13218 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -800,16 +800,6 @@ void AMCLLocalizer::correct(NavState & nav_state) } 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 ------------------- From 5ef22628af4e9ed113344790f86d5cd14f45537b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:14:23 +0100 Subject: [PATCH 031/136] A Star uses obstacle layer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_planner/AStarPlanner.cpp | 43 +++++++++++-------- 1 file changed, 26 insertions(+), 17 deletions(-) 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..57399091 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -296,7 +296,7 @@ std::vector AStarPlanner::a_star_path( if (nm.navcels.empty()) {return {};} - // 1) Locate start & goal navcels (fallback to closest triangle) + // 1) Locate start and goal NavCels (fallback to closest triangle if necessary) size_t sidx_s = 0, sidx_g = 0; NavCelId cid_start = 0, cid_goal = 0; Eigen::Vector3f bary; Eigen::Vector3f hit; @@ -317,7 +317,7 @@ std::vector AStarPlanner::a_star_path( const size_t N = nm.navcels.size(); - // 3) Precompute centroids (2D) for consistent metric and heuristic + // 2) Precompute centroids (used for cost and heuristic) std::vector C(N); for (NavCelId c = 0; c < N; ++c) { const auto cc = nm.navcel_centroid(c); @@ -328,24 +328,36 @@ std::vector AStarPlanner::a_star_path( const auto d = C[a] - C[b]; return static_cast(d.norm()); }; - auto step_cost = [&](NavCelId from, NavCelId to) -> double { - const double dist = static_cast((C[from] - C[to]).norm()); - return dist; + return static_cast((C[from] - C[to]).norm()); + }; + + // 3) Read the "obstacles" layer once and cache occupancy values + std::vector occ(N, FREE_SPACE); + for (NavCelId c = 0; c < N; ++c) { + // If the cell has no stored value, assume FREE_SPACE (0) + occ[c] = nm.layer_get("obstacles", c, FREE_SPACE); + } + auto is_free = [&](NavCelId c) -> bool { + // Strict policy: only 0 (FREE_SPACE) is traversable; any other value blocks + return occ[c] == FREE_SPACE; }; - // 4) A* on triangle graph + // If either start or goal falls in a non-free NavCel, do not plan + if (!is_free(cid_start) || !is_free(cid_goal)) { + return {}; + } + + // 4) Standard A* search on the triangle graph, skipping occupied NavCels 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; 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(); @@ -353,16 +365,13 @@ std::vector AStarPlanner::a_star_path( 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 - // } - for (NavCelId v : nm.navcel_neighbors(u)) { const size_t vidx = static_cast(v); if (vidx >= N) {continue;} + // ---- Occupancy check: skip non-free neighbors ---- + if (!is_free(v)) {continue;} + const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} @@ -372,16 +381,16 @@ std::vector AStarPlanner::a_star_path( parent[v] = u; const double f = tentative + h(v, cid_goal); open.push(Node{v, f}); - in_open[v] = 1; } } } + // If no valid path was found, return an empty list 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]) { geometry_msgs::msg::Pose p; @@ -394,7 +403,7 @@ std::vector AStarPlanner::a_star_path( } std::reverse(path.begin(), path.end()); - // Ensure at least goal pose + // Guarantee at least the goal pose if (path.empty()) {path.push_back(goal);} return path; } From 29baccf301ad43f2935730112a1188fdee8ad316 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:21:02 +0100 Subject: [PATCH 032/136] Remove traces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../NavMapMapsManager.cpp | 6 ------ .../filters/ObstacleFilter.cpp | 18 ------------------ 2 files changed, 24 deletions(-) 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 3ebc978a..409beaf0 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 @@ -228,15 +228,9 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) filter->set_map_resolution(resolution_); filter->update(nav_state); - std::cerr << - "plugin name: " << filter->get_plugin_name() << - "\tlayer name: " << filter->get_layer_name() << - "\tis_adding" << (filter->is_adding_layer() ? "True" : "False") << std::endl; - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); if (filter->is_adding_layer() && navmap_.has_layer(filter->get_layer_name())) { - std::cerr << "Publishin layer " << filter->get_layer_name() << std::endl; 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/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index a1855bbe..6cfa7d5e 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 @@ -49,9 +49,6 @@ ObstacleFilter::on_initialize() void ObstacleFilter::update(::easynav::NavState & nav_state) { - auto t0 = parent_node_->now(); - std::cerr << "ObstacleFilter::update" << std::endl; - if (!nav_state.has("map.navmap")) {return;} if (!nav_state.has("points")) {return;} @@ -60,16 +57,12 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) navmap_.layer_clear(get_layer_name(), 0); - auto t1 = parent_node_->now(); - const auto & points = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) .downsample(0.3) .fuse("map") ->as_points(); - auto t2 = parent_node_->now(); - const float voxel_xy = 0.30f; const float voxel_z = 0.20f; @@ -115,8 +108,6 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) if (z < acc.min_z) {acc.min_z = z;} } - auto t3 = parent_node_->now(); - std::optional last_surface; std::optional<::navmap::NavCelId> last_cid; @@ -171,16 +162,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) } } - - auto t4 = parent_node_->now(); nav_state.set("map.navmap", navmap_); - auto t5 = parent_node_->now(); - - // std::cerr << "t1 = " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; - // std::cerr << "t2 = " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; - // std::cerr << "t3 = " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; - // std::cerr << "t4 = " << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; - // std::cerr << "t5 = " << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; } From b2bfbd5555593a50761cc48343a19f17899b8a15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 08:24:02 +0100 Subject: [PATCH 033/136] Remove commented traces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_maps_manager/filters/ObstacleFilter.cpp | 5 ----- 1 file changed, 5 deletions(-) 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 6cfa7d5e..cebeaff5 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 @@ -121,11 +121,6 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const float cy = (static_cast(key.iy) + 0.5f) * voxel_xy; const float dz = acc.max_z - acc.min_z; - // std::cerr << "[ObstacleFilter] voxel (" << cx << ", " << cy << ") " - // << "vertical_bins=" << acc.z_bins.size() - // << " z_range=[" << acc.min_z << ", " << acc.max_z - // << "] Δz=" << dz << std::endl; - if (acc.z_bins.size() <= 2 && dz <= height_threshold) { continue; } From 1273163dd0135088fc0c4350b3ff087c2b2e84ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:26:50 +0100 Subject: [PATCH 034/136] Update to Occ constants MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_maps_manager/filters/ObstacleFilter.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 cebeaff5..c8c236dc 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 @@ -27,6 +27,7 @@ #include "easynav_common/types/PointPerception.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/ObstacleFilter.hpp" @@ -55,7 +56,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const auto & perceptions = nav_state.get("points"); navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - navmap_.layer_clear(get_layer_name(), 0); + navmap_.layer_clear(get_layer_name(), navmap_ros::FREE_SPACE); const auto & points = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) @@ -151,7 +152,8 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) } if (ok) { - navmap_.layer_set(get_layer_name(), cid, static_cast(255)); + navmap_.layer_set( + get_layer_name(), cid, static_cast(navmap_ros::LETHAL_OBSTACLE)); last_surface = surface_idx; last_cid = cid; } From 68fa782e2dafd217ef1bbe92cf07a1ee30de5cef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:27:17 +0100 Subject: [PATCH 035/136] First functional version of Inflation filter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/InflationFilter.cpp | 147 +++++++++++------- 1 file changed, 87 insertions(+), 60 deletions(-) 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 3c023944..e79104e1 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 @@ -47,6 +47,7 @@ #include "easynav_common/types/PointPerception.hpp" #include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" #include "easynav_navmap_maps_manager/filters/InflationFilter.hpp" @@ -56,17 +57,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, @@ -76,94 +71,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;} @@ -183,44 +202,52 @@ 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_); + "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f", + inflation_radius_, cost_scaling_factor_, inscribed_radius_); return {}; } -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"); + auto nm = nav_state.get<::navmap::NavMap>("map.navmap"); + + const bool ok = inflate_layer_u8( + nm, + "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", nm); } - } // namespace navmap } // namespace easynav + #include PLUGINLIB_EXPORT_CLASS(easynav::navmap::InflationFilter, easynav::navmap::NavMapFilter) From fa8ddc3b6b5aa3e4ab9b46d05b3f24300c2b5874 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:27:42 +0100 Subject: [PATCH 036/136] Path planner aware of obstacles and inflation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_planner/AStarPlanner.cpp | 87 ++++++++++++++----- 1 file changed, 64 insertions(+), 23 deletions(-) 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 57399091..758c7277 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -35,18 +35,13 @@ #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; @@ -287,12 +282,19 @@ 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)); +} + 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 {};} @@ -317,47 +319,87 @@ std::vector AStarPlanner::a_star_path( const size_t N = nm.navcels.size(); - // 2) Precompute centroids (used for cost and heuristic) + // 2) Precompute centroids (used for cost, heuristic, and edge lengths) 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()}; } - auto h = [&](NavCelId a, NavCelId b) -> double { + auto euclid = [&](NavCelId a, NavCelId b) -> double { const auto d = C[a] - C[b]; return static_cast(d.norm()); }; - auto step_cost = [&](NavCelId from, NavCelId to) -> double { - return static_cast((C[from] - C[to]).norm()); - }; - // 3) Read the "obstacles" layer once and cache occupancy values + // 3) Choose cost layer: prefer "inflated_obstacles", fallback to "obstacles" + const std::string cost_layer = + layer_exists(nm, "inflated_obstacles") ? "inflated_obstacles" : "obstacles"; + + // Cache per-NavCel uint8_t cost values (0..255) std::vector occ(N, FREE_SPACE); for (NavCelId c = 0; c < N; ++c) { // If the cell has no stored value, assume FREE_SPACE (0) - occ[c] = nm.layer_get("obstacles", c, FREE_SPACE); + occ[c] = nm.layer_get(cost_layer, c, FREE_SPACE); } - auto is_free = [&](NavCelId c) -> bool { - // Strict policy: only 0 (FREE_SPACE) is traversable; any other value blocks - return occ[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); }; - // If either start or goal falls in a non-free NavCel, do not plan - if (!is_free(cid_start) || !is_free(cid_goal)) { + // 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 {}; } - // 4) Standard A* search on the triangle graph, skipping occupied NavCels + // 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 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* 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 parent(N, std::numeric_limits::max()); + std::vector in_open(N, 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(); @@ -369,8 +411,8 @@ std::vector AStarPlanner::a_star_path( const size_t vidx = static_cast(v); if (vidx >= N) {continue;} - // ---- Occupancy check: skip non-free neighbors ---- - if (!is_free(v)) {continue;} + // Skip non-traversable neighbors (lethal or unknown) + if (!traversable(v)) {continue;} const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} @@ -381,11 +423,11 @@ std::vector AStarPlanner::a_star_path( parent[v] = u; const double f = tentative + h(v, cid_goal); open.push(Node{v, f}); + in_open[v] = 1; } } } - // If no valid path was found, return an empty list if (!std::isfinite(g[cid_goal])) { return {}; } @@ -403,7 +445,6 @@ std::vector AStarPlanner::a_star_path( } std::reverse(path.begin(), path.end()); - // Guarantee at least the goal pose if (path.empty()) {path.push_back(goal);} return path; } From e46d6121db18ae195226a883c25f2d5545b2ead2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:27:54 +0100 Subject: [PATCH 037/136] Update sheets MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/README.md | 5 +- .../easynav_serest_controller/README.md | 3 +- .../easynav_simple_controller/README.md | 8 +- controllers/easynav_vff_controller/README.md | 5 +- .../easynav_costmap_localizer/README.md | 4 +- localizers/easynav_gps_localizer/README.md | 4 +- localizers/easynav_navmap_localizer/README.md | 4 +- localizers/easynav_simple_localizer/README.md | 4 +- .../easynav_bonxai_maps_manager/README.md | 4 +- .../easynav_costmap_maps_manager/README.md | 4 +- .../easynav_navmap_maps_manager/README.md | 98 +++++++++++++++---- .../easynav_simple_maps_manager/README.md | 4 +- planners/easynav_costmap_planner/README.md | 4 +- planners/easynav_navmap_planner/README.md | 57 +++++++---- planners/easynav_simple_planner/README.md | 4 +- 15 files changed, 158 insertions(+), 54 deletions(-) diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 655e8568..4daac078 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -1,6 +1,6 @@ # easynav_mppi_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)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-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 @@ -13,9 +13,10 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig ## 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` diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index f5d6fa49..0b2e200c 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -13,9 +13,10 @@ A SeReST (Smooth Error-Responsive Speed and Turning) controller for path trackin ## 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` diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index c01e2eb2..bf139bfc 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -1,6 +1,7 @@ # easynav_simple_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)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-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 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. @@ -12,9 +13,10 @@ Simple path-following controller that uses PID controllers and a look-ahead refe ## 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) | +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) - **Plugin Name:** `easynav_simple_controller/SimpleController` diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index ab2314ce..e6cd3315 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -1,6 +1,7 @@ # 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)](#) +[![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 Easy Navigation: VFF Controller package. @@ -12,6 +13,8 @@ Easy Navigation: VFF Controller package. ## 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/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8d3ae420..c2f8ce66 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for prediction. @@ -12,6 +12,8 @@ AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for predi ## 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/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index a989cfa3..b3b44698 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. @@ -12,6 +12,8 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- ## 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/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index 93ce6657..c2229864 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 Easy Navigation: nAVmAP Localizer package. @@ -12,6 +12,8 @@ Easy Navigation: nAVmAP Localizer package. ## 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/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 04505e89..2de8ce67 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 AMCL-style localizer using a simple 2D grid-like map for scoring and odometry for prediction. @@ -12,6 +12,8 @@ AMCL-style localizer using a simple 2D grid-like map for scoring and odometry fo ## 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/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index 4df2fba2..bb3c03c9 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 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. @@ -12,6 +12,8 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) ## 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/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index e3a54d13..195f710a 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 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. @@ -19,6 +19,8 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends ## 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/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 1f7ce673..2a45f1cb 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -1,66 +1,124 @@ # 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)](#) +[![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 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_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 413125aa..8c002e1e 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -1,6 +1,6 @@ # 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)](#) +[![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 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. @@ -14,6 +14,8 @@ At the heart of this stack is the SimpleMap data structure. It represents the en ## 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_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index fff2666f..dbea0d9e 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) | diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 58d80e1b..fa7170c6 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). | - +| `.layer` | `string` | `"inflated_obstacles"` | Name of the NavMap layer containing 8-bit costs (`FREE_SPACE=0`, `INSCRIBED_INFLATED_OBSTACLE=253`, `LETHAL_OBSTACLE=254`). | +| `.cost_factor` | `double` | `2.0` | Multiplicative weight for geometric distance; values > 1 increase the relative importance of distance. | +| `.inflation_penalty` | `double` | `5.0` | Additional penalty proportional to the normalized cell cost; higher values keep the path farther from inflated areas. | +| `.continuous_replan` | `bool` | `true` | If true, recomputes the path whenever `NavState` updates; if false, plans once per goal. | ## Interfaces (Topics and Services) @@ -40,20 +62,19 @@ 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_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) | From 9300b96bc2b7ecce7ea34ad65482d2a98e5abb47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 13 Nov 2025 08:35:39 +0100 Subject: [PATCH 038/136] Update Readme MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- planners/easynav_navmap_planner/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index fa7170c6..164419dc 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -73,8 +73,7 @@ This plugin does not create subscriptions or services directly; it retrieves all | `path` | `nav_msgs::msg::Path` | **Write** | Output path, computed as the lowest-cost route. | ## TF Frames -The planner assumes frame consistency between NavMap, robot pose, and goals. -No TF lookups are performed internally. +The planner assumes frame consistency between NavMap, robot pose, and goals. No TF lookups are performed internally. ## License GPL-3.0-only From 02aa5fc3799697ec92778dbdade38e41ef9eb6f4 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 13 Nov 2025 18:43:51 +0100 Subject: [PATCH 039/136] MPC was tunned --- .../easynav_mpc_controller/MPCController.hpp | 9 +- .../easynav_mpc_controller/MPCController.cpp | 87 +++++++++---------- 2 files changed, 47 insertions(+), 49 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 384586d7..196aa781 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,7 @@ struct MPCParameters{ Eigen::Matrix2d Q; Eigen::Matrix2d R; Eigen::Matrix2d Rd; + double qtheta; int N; double dt; }; @@ -61,9 +63,10 @@ class MPCController : public ControllerMethodBase double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. private: - Eigen::Matrix2d Q_ {{1.0, 0.0},{0.0, 1.0}}; - Eigen::Matrix2d R_ {{0.01, 0.0},{0.0, 0.01}}; - Eigen::Matrix2d Rd_ {{0.01, 0.0},{0.0, 1.0}}; + Eigen::Matrix2d Q_ {{2.0, 0.0},{0.0, 2.0}}; + Eigen::Matrix2d R_ {{0.05, 0.0},{0.0, 0.05}}; + Eigen::Matrix2d Rd_ {{0.1, 0.0},{0.0, 0.1}}; + double qtheta_ {0.5}; geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. }; diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 666a1474..20476247 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -40,20 +40,13 @@ cost_function(const std::vector &u, std::vector &grad, void *dat Eigen::Vector3d x = params->x0; int N = params->N; double dt = params->dt; + double qtheta = params->qtheta; double cost = 0.0; Eigen::Matrix2d R = params->R; Eigen::Matrix2d Q = params->Q; Eigen::Matrix2d Rd = params->Rd; - if (!grad.empty()) { - grad.assign(u.size(), 0.0); - } - - // std::cout << "====="<< std::endl; - // std::cout << "u: "<< u[0] << std::endl; - // std::cout << "N: "<< params->N << std::endl; - for (int i = 0; i < N; ++i) { double v = u[2*i]; double w = u[2*i + 1]; @@ -67,21 +60,20 @@ cost_function(const std::vector &u, std::vector &grad, void *dat dw = 0.0; } - // std::cout << "===== "<< std::endl; - // std::cout << "x: "<< x << std::endl; - // std::cout << "v: "<< v << std::endl; - // std::cout << "w: "<< w << std::endl; - // std::cout << "dt: "<< dt << std::endl; - x = kinematic_model(x, v, w, dt); Eigen::Vector2d pos = x.head<2>(); - Eigen::Vector2d error = pos - params->goal; + Eigen::Vector2d error = params->goal - pos; + double error_theta = (atan2((error[1]),(error[0]))) - x[2]; Eigen::Vector2d uk(v, w); Eigen::Vector2d duk(dv, dw); - - // cost += (uk.transpose()*R)*uk + (error.transpose()*Q)*error + (duk.transpose()*Rd)*duk; - cost += uk.dot(R * uk) + error.dot(Q * error) + duk.dot(Rd * duk); + + // 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; @@ -118,8 +110,6 @@ MPCController::on_initialize() node->get_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); node->get_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); - - return {}; } @@ -135,7 +125,6 @@ MPCController::update_rt(NavState & nav_state) if (path.poses.empty()) { // If the path is empty, stop the robot - std::cout << "Path Empty" << std::endl; cmd_vel_.header.frame_id = path.header.frame_id; cmd_vel_.header.stamp = get_node()->now(); cmd_vel_.twist.linear.x = 0.0; @@ -145,7 +134,6 @@ MPCController::update_rt(NavState & nav_state) } int num_elements = path.poses.size(); - std::cout << "num_elemnts" << std::endl; const auto pose = nav_state.get("robot_pose").pose.pose; double roll_, pitch_, yaw_; @@ -160,11 +148,15 @@ MPCController::update_rt(NavState & nav_state) // MPC Code - std::cout << "MPC elements" << std::endl; - MPCParameters params; params.x0 = {pose.position.x, pose.position.y, yaw_}; - const auto &last_pose = path.poses[num_elements - 1].pose.position; + 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; params.goal = Eigen::Vector2d(static_cast(last_pose.x), static_cast(last_pose.y)); params.N = horizon_steps_; @@ -172,36 +164,39 @@ MPCController::update_rt(NavState & nav_state) params.R = R_; params.Q = Q_; params.Rd = Rd_; + params.qtheta = qtheta_; double minf; std::vector u(2*horizon_steps_, 0.0); - // nlopt::opt opt(nlopt::LD_SLSQP, static_cast(u.size())); nlopt::opt opt(nlopt::LN_COBYLA, static_cast(u.size())); opt.set_min_objective(nlopt_cost_callback, ¶ms); - std::cout << "Set cost function" << std::endl; - - std::vector lb(2*horizon_steps_, -max_lin_vel_); - std::vector ub(2*horizon_steps_, max_lin_vel_); + 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-4); - - std::cout << "Configurated optimizer" << std::endl; - - nlopt::result result = opt.optimize(u, minf); + opt.set_xtol_rel(1e-6); + opt.set_ftol_rel(1e-8); + opt.set_maxeval(100); - // try { - // nlopt::result result = opt.optimize(u, minf); - // std::cout << "Optimizo" << std::endl; - // if (result != nlopt::SUCCESS) - // { - // std::cerr << "Optimization Unsuccessful " << std::endl; - // } - // } catch (std::exception &e) { - // std::cerr << "Optimization Error: " << e.what() << std::endl; - // } - std::cout << u[0] << ", " << u[1] << std::endl; + try { + nlopt::result result = opt.optimize(u, minf); + // if (result != nlopt::SUCCESS) + // { + // std::cerr << "Optimization Unsuccessful " << std::endl; + // std::cout << "Result: " << result << std::endl; + // } else { + // std::cerr << "Optimization Successful " << std::endl; + // } + } catch (std::exception &e) { + std::cerr << "Optimization Error: " << e.what() << std::endl; + } // Publish the computed velocity command cmd_vel_.header.frame_id = path.header.frame_id; From 525ba4ddf384190030959482b26ada7d763304c6 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 13 Nov 2025 18:54:56 +0100 Subject: [PATCH 040/136] Readme was updated --- controllers/easynav_mpc_controller/README.md | 9 +-- .../easynav_mpc_controller/MPCController.hpp | 9 +-- .../easynav_mpc_controller/MPCController.cpp | 69 ++++++++++--------- 3 files changed, 42 insertions(+), 45 deletions(-) diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md index 7f814cf6..59b2a9d9 100644 --- a/controllers/easynav_mpc_controller/README.md +++ b/controllers/easynav_mpc_controller/README.md @@ -1,6 +1,6 @@ # easynav_mpc_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)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) +[![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) ## Description @@ -13,9 +13,7 @@ A Model Predictive Controller (MPC) implementation for Easy Navigation. ## 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) | -| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | ## Plugin (pluginlib) - **Plugin Name:** `easynav_mpc_controller/MPCController` @@ -38,10 +36,7 @@ All parameters are declared under the plugin namespace, i.e., `//easyn ## 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 | +This package does not create topic subscriptions or publications. ### Services diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 196aa781..ed9f7085 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -25,7 +25,8 @@ #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" -struct MPCParameters{ +struct MPCParameters +{ Eigen::Vector2d goal; Eigen::Vector3d x0; Eigen::Matrix2d Q; @@ -63,9 +64,9 @@ class MPCController : public ControllerMethodBase double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. private: - Eigen::Matrix2d Q_ {{2.0, 0.0},{0.0, 2.0}}; - Eigen::Matrix2d R_ {{0.05, 0.0},{0.0, 0.05}}; - Eigen::Matrix2d Rd_ {{0.1, 0.0},{0.0, 0.1}}; + Eigen::Matrix2d Q_ {{2.0, 0.0}, {0.0, 2.0}}; + Eigen::Matrix2d R_ {{0.05, 0.0}, {0.0, 0.05}}; + Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; double qtheta_ {0.5}; geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 20476247..d6395879 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -23,7 +23,7 @@ #include "easynav_mpc_controller/MPCController.hpp" Eigen::Vector3d -kinematic_model(const Eigen::Vector3d &x, double v, double w, double dt) +kinematic_model(const Eigen::Vector3d & x, double v, double w, double dt) { Eigen::Vector3d x_k1; x_k1[0] = x[0] + v * cos(x[2]) * dt; @@ -32,10 +32,10 @@ kinematic_model(const Eigen::Vector3d &x, double v, double w, double dt) return x_k1; } -double -cost_function(const std::vector &u, std::vector &grad, void *data) +double +cost_function(const std::vector & u, std::vector & grad, void *data) { - MPCParameters *params = reinterpret_cast(data); + MPCParameters *params = reinterpret_cast(data); Eigen::Vector3d x = params->x0; int N = params->N; @@ -46,15 +46,14 @@ cost_function(const std::vector &u, std::vector &grad, void *dat Eigen::Matrix2d R = params->R; Eigen::Matrix2d Q = params->Q; Eigen::Matrix2d Rd = params->Rd; - + for (int i = 0; i < N; ++i) { - double v = u[2*i]; - double w = u[2*i + 1]; + 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]; + 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; @@ -64,25 +63,27 @@ cost_function(const std::vector &u, std::vector &grad, void *dat Eigen::Vector2d pos = x.head<2>(); Eigen::Vector2d error = params->goal - pos; - double error_theta = (atan2((error[1]),(error[0]))) - x[2]; + double error_theta = (atan2((error[1]), (error[0]))) - x[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; + 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; + 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; } -static double nlopt_cost_callback(const std::vector &x, - std::vector &grad, - void *data) +static double nlopt_cost_callback( + const std::vector & x, + std::vector & grad, + void *data) { return cost_function(x, grad, data); } @@ -156,7 +157,7 @@ MPCController::update_rt(NavState & nav_state) } else { local_horizon = num_elements - 1; } - const auto &last_pose = path.poses[local_horizon].pose.position; + const auto & last_pose = path.poses[local_horizon].pose.position; params.goal = Eigen::Vector2d(static_cast(last_pose.x), static_cast(last_pose.y)); params.N = horizon_steps_; @@ -166,27 +167,27 @@ MPCController::update_rt(NavState & nav_state) params.Rd = Rd_; params.qtheta = qtheta_; double minf; - std::vector u(2*horizon_steps_, 0.0); + std::vector u(2 * horizon_steps_, 0.0); nlopt::opt opt(nlopt::LN_COBYLA, static_cast(u.size())); opt.set_min_objective(nlopt_cost_callback, ¶ms); - 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_; + 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-6); opt.set_ftol_rel(1e-8); opt.set_maxeval(100); - + try { - nlopt::result result = opt.optimize(u, minf); + nlopt::result result = opt.optimize(u, minf); // if (result != nlopt::SUCCESS) // { // std::cerr << "Optimization Unsuccessful " << std::endl; @@ -194,8 +195,8 @@ MPCController::update_rt(NavState & nav_state) // } else { // std::cerr << "Optimization Successful " << std::endl; // } - } catch (std::exception &e) { - std::cerr << "Optimization Error: " << e.what() << std::endl; + } catch (std::exception & e) { + std::cerr << "Optimization Error: " << e.what() << std::endl; } // Publish the computed velocity command From 17c17163fd5e6c2f031129f3441909297c07701e Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 13 Nov 2025 19:00:06 +0100 Subject: [PATCH 041/136] Readme was updated --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index ea126c17..e06b219c 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,7 @@ 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) | --- From 9855e3948ea9db31f6a9afff38e440635fd609c2 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 14 Nov 2025 14:47:10 +0100 Subject: [PATCH 042/136] CI updated --- .github/workflows/rolling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 651aa501..8a697ab1 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -29,7 +29,7 @@ jobs: - 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_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 From aad6295ef8c2f7c00f22abb3569df3c99ff46ca7 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 14 Nov 2025 15:05:11 +0100 Subject: [PATCH 043/136] CI updated --- controllers/easynav_mpc_controller/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 85209052..38d7a921 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -8,6 +8,7 @@ GPL-3.0-only ament_cmake + nlopt easynav_common easynav_core @@ -15,7 +16,7 @@ tf2_ros geometry_msgs nav_msgs - NLopt + nlopt rclcpp_lifecycle easynav_simple_common From 87f7f1584f6ca6546b97f91cd81736c79de05836 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 14 Nov 2025 16:15:08 +0100 Subject: [PATCH 044/136] CI Updated --- controllers/easynav_mpc_controller/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 38d7a921..34c3ca91 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -16,7 +16,6 @@ tf2_ros geometry_msgs nav_msgs - nlopt rclcpp_lifecycle easynav_simple_common From f7d72373237d11c68f5a351b6776683933c7f0ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 14 Nov 2025 19:07:47 +0100 Subject: [PATCH 045/136] Init pose for RViz2 compliant MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_localizer/README.md | 3 +- .../AMCLLocalizer.hpp | 11 ++ .../AMCLLocalizer.cpp | 156 ++++++++++++++++++ 3 files changed, 169 insertions(+), 1 deletion(-) diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 8d3ae420..854a24ad 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -47,7 +47,8 @@ All parameters are declared under the plugin namespace, i.e., `//easyn ### 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 | 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..6381a32f 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -164,6 +164,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 +174,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 * 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 be56dac4..f84fdbbd 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -266,6 +266,9 @@ 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(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); @@ -326,6 +329,159 @@ 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(); + + // Check expected frame + const std::string expected_frame = get_tf_prefix() + std::string("map"); + 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() { From 2842e46019d29ad597b177b6a5f40cd247180b24 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 15 Nov 2025 07:18:53 +0100 Subject: [PATCH 046/136] Fix CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/rolling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 651aa501..54e88333 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -18,7 +18,7 @@ jobs: fail-fast: false steps: - name: Repo checkout - uses: actions/checkout@v4 + uses: actions/checkout@v6-beta with: ref: rolling - name: Setup ROS 2 From 0037766a7d8104a3d6106cedd1f7559c9ebbdc6c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 15 Nov 2025 07:43:39 +0100 Subject: [PATCH 047/136] Fix CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/rolling.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 54e88333..052f3add 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -31,7 +31,6 @@ jobs: 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: rolling - ref: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: true colcon-defaults: | From daea5322e6d415a034663c87c4be288b043f1edb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 15 Nov 2025 07:48:29 +0100 Subject: [PATCH 048/136] Fix CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/humble.yaml | 3 -- .github/workflows/humble_cron.yaml | 50 +++++++++++++++++++++++++++++ .github/workflows/jazzy.yaml | 1 - .github/workflows/jazzy_cron.yaml | 50 +++++++++++++++++++++++++++++ .github/workflows/kilted.yaml | 3 -- .github/workflows/kilted_cron.yaml | 50 +++++++++++++++++++++++++++++ .github/workflows/rolling.yaml | 2 -- .github/workflows/rolling_cron.yaml | 50 +++++++++++++++++++++++++++++ 8 files changed, 200 insertions(+), 9 deletions(-) create mode 100644 .github/workflows/humble_cron.yaml create mode 100644 .github/workflows/jazzy_cron.yaml create mode 100644 .github/workflows/kilted_cron.yaml create mode 100644 .github/workflows/rolling_cron.yaml diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 656ea9de..b7d97f4b 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -7,8 +7,6 @@ on: push: branches: - humble - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: runs-on: ${{ matrix.os }} @@ -31,7 +29,6 @@ jobs: 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: true colcon-defaults: | diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml new file mode 100644 index 00000000..d603e839 --- /dev/null +++ b/.github/workflows/humble_cron.yaml @@ -0,0 +1,50 @@ +name: humble + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-22.04] + fail-fast: false + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + ref: humble + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + with: + required-ros-distributions: humble + + - 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: true + 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 fa433e5f..33198952 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -31,7 +31,6 @@ jobs: 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: jazzy - ref: jazzy vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: true colcon-defaults: | diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml new file mode 100644 index 00000000..f556576f --- /dev/null +++ b/.github/workflows/jazzy_cron.yaml @@ -0,0 +1,50 @@ +name: jazzy + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + 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 + target-ros2-distro: jazzy + ref: jazzy + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: true + 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 bcee3c4d..42472c23 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -7,8 +7,6 @@ on: push: branches: - kilted - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: runs-on: ${{ matrix.os }} @@ -31,7 +29,6 @@ jobs: 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: kilted - ref: kilted vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: true colcon-defaults: | diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml new file mode 100644 index 00000000..79c00ef6 --- /dev/null +++ b/.github/workflows/kilted_cron.yaml @@ -0,0 +1,50 @@ +name: kilted + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - name: Repo checkout + uses: actions/checkout@v4 + with: + 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 + target-ros2-distro: kilted + ref: kilted + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: true + 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 052f3add..d71cd0fe 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -7,8 +7,6 @@ on: push: branches: - rolling - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: runs-on: ${{ matrix.os }} diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml new file mode 100644 index 00000000..75f47b44 --- /dev/null +++ b/.github/workflows/rolling_cron.yaml @@ -0,0 +1,50 @@ +name: rolling + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - name: Repo checkout + 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 + target-ros2-distro: rolling + ref: rolling + vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos + skip-tests: true + 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 From e08731298bc63a500b54428c510df5733bc72242 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 15 Nov 2025 08:01:36 +0100 Subject: [PATCH 049/136] Fix CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/humble.yaml | 2 +- .github/workflows/humble_cron.yaml | 2 +- .github/workflows/jazzy.yaml | 2 +- .github/workflows/jazzy_cron.yaml | 2 +- .github/workflows/kilted.yaml | 2 +- .github/workflows/kilted_cron.yaml | 2 +- .github/workflows/rolling.yaml | 2 +- .github/workflows/rolling_cron.yaml | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index b7d97f4b..c98350fc 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -30,7 +30,7 @@ jobs: 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: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml index d603e839..f195c9fa 100644 --- a/.github/workflows/humble_cron.yaml +++ b/.github/workflows/humble_cron.yaml @@ -27,7 +27,7 @@ jobs: target-ros2-distro: humble ref: humble vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 33198952..bb185e0b 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -32,7 +32,7 @@ jobs: 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: 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 index f556576f..38ebc4df 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.github/workflows/jazzy_cron.yaml @@ -27,7 +27,7 @@ jobs: target-ros2-distro: jazzy ref: jazzy vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 42472c23..4e34d3b7 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -30,7 +30,7 @@ jobs: 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: 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 index 79c00ef6..6167d6ce 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.github/workflows/kilted_cron.yaml @@ -27,7 +27,7 @@ jobs: target-ros2-distro: kilted ref: kilted vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index d71cd0fe..4ac207f7 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -30,7 +30,7 @@ jobs: 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: 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 index 75f47b44..40abdff2 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.github/workflows/rolling_cron.yaml @@ -27,7 +27,7 @@ jobs: target-ros2-distro: rolling ref: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-tests: true + skip-tests: false colcon-defaults: | { "build": { From 68ff6ef287f9070374815bc76f02efd115820b04 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 17 Nov 2025 20:21:36 +0100 Subject: [PATCH 050/136] Adaptation to EasyNavigation#71 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_mppi_controller/MPPIController.cpp | 4 ++-- .../src/easynav_serest_controller/SerestController.cpp | 4 ++-- .../src/easynav_vff_controller/VffController.cpp | 5 +++-- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 4 ++-- .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 4 ++-- .../easynav_costmap_maps_manager/filters/ObstacleFilter.cpp | 2 +- .../easynav_navmap_maps_manager/filters/ObstacleFilter.cpp | 2 +- .../easynav_octomap_maps_manager/filters/ObstacleFilter.cpp | 2 +- .../src/easynav_simple_maps_manager/SimpleMapsManager.cpp | 2 +- 9 files changed, 15 insertions(+), 14 deletions(-) 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 2ea5c5b3..8879738f 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -173,9 +173,9 @@ MPPIController::update_rt(NavState & nav_state) const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) .fuse("map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) - ->downsample(0.1) + .downsample(0.1) .as_points(); if (filtered.empty()) { 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 669a2d2d..e62f5162 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -304,10 +304,10 @@ SerestController::closest_obstacle_distance( auto fused = PointPerceptionsOpsView(perceptions) .downsample(0.3) .fuse(get_tf_prefix() + "base_link") - ->filter({-dist_search_radius_, -dist_search_radius_, NAN}, + .filter({-dist_search_radius_, -dist_search_radius_, NAN}, {dist_search_radius_, dist_search_radius_, 2.0}) .collapse({NAN, NAN, 0.1}) - ->downsample(0.3) + .downsample(0.3) .as_points(); double min_dist = std::numeric_limits::infinity(); 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 d03189eb..f3f6b270 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -264,9 +264,10 @@ void VffController::update_rt(NavState & nav_state) PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0}) .fuse(get_tf_prefix() + "base_link") - ->filter({obstacle_detection_x_min_, obstacle_detection_y_min_, obstacle_detection_z_min_}, + .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"); 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 f84fdbbd..c653b573 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -583,9 +583,9 @@ AMCLLocalizer::correct(NavState & nav_state) const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.getResolution()) .fuse(get_tf_prefix() + "base_footprint") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) - ->downsample(map_static.getResolution()) + .downsample(map_static.getResolution()) .as_points(); if (filtered.empty()) { 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 af398c5b..cc5c05f6 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -390,9 +390,9 @@ void AMCLLocalizer::correct(NavState & nav_state) const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.resolution()) .fuse(get_tf_prefix() + "base_footprint") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) - ->downsample(map_static.resolution()) + .downsample(map_static.resolution()) .as_points(); if (filtered.empty()) { 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 f6623a47..9fc6d254 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 @@ -60,7 +60,7 @@ ObstacleFilter::update(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map.getResolution()) .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); for (const auto & p : fused) { 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 c8c236dc..3f4a29c6 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 @@ -62,7 +62,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) .downsample(0.3) .fuse("map") - ->as_points(); + .as_points(); const float voxel_xy = 0.30f; const float voxel_z = 0.20f; 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 8d6671fc..03d82265 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 @@ -67,7 +67,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(get_map_resolution()) .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); size_t sidx = 0; 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 310a2261..410c8a33 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 @@ -156,7 +156,7 @@ SimpleMapsManager::update(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map_.resolution()) .fuse(get_tf_prefix() + "map") - ->filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); for (const auto & p : fused) { From 5285226a8af97822fc8d12f0d4e30ff47f4bd6b2 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 19 Nov 2025 19:21:06 +0100 Subject: [PATCH 051/136] Visualization Marker was added --- .../easynav_mpc_controller/MPCController.hpp | 10 ++++ .../easynav_mpc_controller/MPCController.cpp | 48 ++++++++++++++++++- 2 files changed, 57 insertions(+), 1 deletion(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index ed9f7085..f9120ef3 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -17,10 +17,13 @@ #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 "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" @@ -57,12 +60,19 @@ class MPCController : public ControllerMethodBase /// \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 pose Eigen::Vector3d of the robot + /// \param best_path Path generated by MPC + void publish_mpc_path(const Eigen::Vector3d & pose, const std::vector & best_path); + protected: int horizon_steps_{5}; ///< Prediction horizon for MPC. double dt_{0.1}; ///< Time step for MPC. double max_lin_vel_{1.5}; ///< Maximum linear velocity for MPC. double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. + rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path markers. + private: Eigen::Matrix2d Q_ {{2.0, 0.0}, {0.0, 2.0}}; Eigen::Matrix2d R_ {{0.05, 0.0}, {0.0, 0.05}}; diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index d6395879..2ed21373 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -111,9 +111,52 @@ MPCController::on_initialize() node->get_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); node->get_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + mpc_path_pub_ = + node->create_publisher("/mpc/path", 10); + return {}; } +void +MPCController::publish_mpc_path(const Eigen::Vector3d & pose, const std::vector & best_vel) +{ + visualization_msgs::msg::MarkerArray path; + visualization_msgs::msg::Marker points; + points.header.frame_id = "map"; + points.header.stamp = rclcpp::Clock().now(); + points.ns = "mpc_path"; + points.id = 0; + points.type = visualization_msgs::msg::Marker::LINE_STRIP; + points.action = visualization_msgs::msg::Marker::ADD; + points.scale.x = 0.05; + points.color.r = 1.0; + points.color.g = 0.0; + points.color.b = 0.0; + points.color.a = 0.8; + + Eigen::Vector3d x; + std::cerr << "====" << std::endl; + std::cerr << "Pose.X: " << pose[0] + << " Pose.Y: " << pose[1] <publish(path); +} + void MPCController::update_rt(NavState & nav_state) { @@ -184,7 +227,7 @@ MPCController::update_rt(NavState & nav_state) opt.set_upper_bounds(ub); opt.set_xtol_rel(1e-6); opt.set_ftol_rel(1e-8); - opt.set_maxeval(100); + opt.set_maxeval(250); try { nlopt::result result = opt.optimize(u, minf); @@ -206,6 +249,9 @@ MPCController::update_rt(NavState & nav_state) cmd_vel_.twist.angular.z = u[1]; nav_state.set("cmd_vel", cmd_vel_); + + // Publish the path + publish_mpc_path(params.x0, u); } } // namespace easynav From 9d5828e3d2ff18384b71a6a3516ff82e4312b624 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 19 Nov 2025 23:35:17 +0100 Subject: [PATCH 052/136] MPC Controller minimizes path following --- .../easynav_mpc_controller/MPCController.hpp | 9 ++++--- .../easynav_mpc_controller/MPCController.cpp | 27 ++++++++++++------- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index f9120ef3..c3641ce1 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -70,14 +70,15 @@ class MPCController : public ControllerMethodBase double dt_{0.1}; ///< Time step for MPC. 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 rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path markers. private: - Eigen::Matrix2d Q_ {{2.0, 0.0}, {0.0, 2.0}}; - Eigen::Matrix2d R_ {{0.05, 0.0}, {0.0, 0.05}}; - Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; - double qtheta_ {0.5}; + Eigen::Matrix2d Q_ {{5.0, 0.0}, {0.0, 5.0}}; ///< Tracking Cost + Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost + Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost + double qtheta_ {2.0}; geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. }; diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 2ed21373..540b5736 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -105,11 +105,13 @@ MPCController::on_initialize() node->declare_parameter(plugin_name + ".dt", dt_); 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->get_parameter(plugin_name + ".horizon_steps", horizon_steps_); node->get_parameter(plugin_name + ".dt", dt_); 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_); mpc_path_pub_ = node->create_publisher("/mpc/path", 10); @@ -161,7 +163,9 @@ void MPCController::update_rt(NavState & nav_state) { if (!nav_state.has("path") || !nav_state.has("robot_pose")) { - std::cout << "No Path or No robot pose" << std::endl; + if(verbose_) { + std::cout << "No Path or No robot pose" << std::endl; + } return; } @@ -226,18 +230,21 @@ MPCController::update_rt(NavState & nav_state) opt.set_lower_bounds(lb); opt.set_upper_bounds(ub); opt.set_xtol_rel(1e-6); - opt.set_ftol_rel(1e-8); - opt.set_maxeval(250); + opt.set_ftol_rel(1e-6); + opt.set_maxeval(750); try { nlopt::result result = opt.optimize(u, minf); - // if (result != nlopt::SUCCESS) - // { - // std::cerr << "Optimization Unsuccessful " << std::endl; - // std::cout << "Result: " << result << std::endl; - // } else { - // std::cerr << "Optimization Successful " << std::endl; - // } + if(verbose_) { + if (result != nlopt::SUCCESS) + { + std::cerr << "Optimization Stopped " << std::endl; + std::cout << "Result: " << result << std::endl; + } else { + std::cerr << "Optimization Successful " << std::endl; + } + } + } catch (std::exception & e) { std::cerr << "Optimization Error: " << e.what() << std::endl; } From 098c834584e81f0d0004ef6329154ee57c9853a1 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 20 Nov 2025 01:14:41 +0100 Subject: [PATCH 053/136] MPC with differential model is working --- .github/workflows/rolling.yaml | 3 ++ .../easynav_mpc_controller/MPCController.hpp | 7 +-- .../easynav_mpc_controller/MPCController.cpp | 47 +++++++++---------- 3 files changed, 29 insertions(+), 28 deletions(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 70451bb4..38ae0a9c 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -24,6 +24,9 @@ jobs: with: required-ros-distributions: rolling + - name: Install nlopt + run: sudo apt install -y libnlopt0 + - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index c3641ce1..14eaa9e8 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -32,6 +32,7 @@ struct MPCParameters { Eigen::Vector2d goal; Eigen::Vector3d x0; + Eigen::Vector3d theta0; Eigen::Matrix2d Q; Eigen::Matrix2d R; Eigen::Matrix2d Rd; @@ -63,7 +64,7 @@ class MPCController : public ControllerMethodBase /// \brief Publishes the selected path by MPC controller /// \param pose Eigen::Vector3d of the robot /// \param best_path Path generated by MPC - void publish_mpc_path(const Eigen::Vector3d & pose, const std::vector & best_path); + void publish_mpc_path(const Eigen::Vector3d & position, const Eigen::Vector3d & orientation, const std::vector & best_path); protected: int horizon_steps_{5}; ///< Prediction horizon for MPC. @@ -75,10 +76,10 @@ class MPCController : public ControllerMethodBase rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path markers. private: - Eigen::Matrix2d Q_ {{5.0, 0.0}, {0.0, 5.0}}; ///< Tracking Cost + Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost - double qtheta_ {2.0}; + double qtheta_ {3.0}; geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. }; diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 540b5736..953270b3 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -23,12 +23,12 @@ #include "easynav_mpc_controller/MPCController.hpp" Eigen::Vector3d -kinematic_model(const Eigen::Vector3d & x, double v, double w, double dt) +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(x[2]) * dt; - x_k1[1] = x[1] + v * sin(x[2]) * dt; - x_k1[2] = x[2] + w * dt; + 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; } @@ -37,7 +37,9 @@ cost_function(const std::vector & u, std::vector & grad, void *d { MPCParameters *params = reinterpret_cast(data); - Eigen::Vector3d x = params->x0; + Eigen::Vector3d position = params->x0; + Eigen::Vector3d orientation = params->theta0; + Eigen::Vector3d state; int N = params->N; double dt = params->dt; double qtheta = params->qtheta; @@ -59,11 +61,11 @@ cost_function(const std::vector & u, std::vector & grad, void *d dw = 0.0; } - x = kinematic_model(x, v, w, dt); + state = kinematic_model(position, orientation, v, w, dt); - Eigen::Vector2d pos = x.head<2>(); + Eigen::Vector2d pos = state.head<2>(); Eigen::Vector2d error = params->goal - pos; - double error_theta = (atan2((error[1]), (error[0]))) - x[2]; + double error_theta = (atan2((error[1]), (error[0]))) - state[2]; Eigen::Vector2d uk(v, w); Eigen::Vector2d duk(dv, dw); @@ -120,7 +122,7 @@ MPCController::on_initialize() } void -MPCController::publish_mpc_path(const Eigen::Vector3d & pose, const std::vector & best_vel) +MPCController::publish_mpc_path(const Eigen::Vector3d & position, const Eigen::Vector3d & orientation, const std::vector & best_vel) { visualization_msgs::msg::MarkerArray path; visualization_msgs::msg::Marker points; @@ -136,22 +138,16 @@ MPCController::publish_mpc_path(const Eigen::Vector3d & pose, const std::vector< points.color.b = 0.0; points.color.a = 0.8; - Eigen::Vector3d x; - std::cerr << "====" << std::endl; - std::cerr << "Pose.X: " << pose[0] - << " Pose.Y: " << pose[1] < horizon_steps_) { local_horizon = horizon_steps_; @@ -229,9 +226,9 @@ MPCController::update_rt(NavState & nav_state) } opt.set_lower_bounds(lb); opt.set_upper_bounds(ub); - opt.set_xtol_rel(1e-6); - opt.set_ftol_rel(1e-6); - opt.set_maxeval(750); + opt.set_xtol_rel(1e-3); + opt.set_ftol_rel(1e-3); + // opt.set_maxeval(1000); try { nlopt::result result = opt.optimize(u, minf); @@ -258,7 +255,7 @@ MPCController::update_rt(NavState & nav_state) nav_state.set("cmd_vel", cmd_vel_); // Publish the path - publish_mpc_path(params.x0, u); + publish_mpc_path(params.x0, params.theta0, u); } } // namespace easynav From f789ada5424926fc6d9992b3458fb604c5fc88ec Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 20 Nov 2025 01:23:06 +0100 Subject: [PATCH 054/136] CI updated --- .github/workflows/rolling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 38ae0a9c..e65e65c5 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -25,7 +25,7 @@ jobs: required-ros-distributions: rolling - name: Install nlopt - run: sudo apt install -y libnlopt0 + run: sudo apt install -y libnlopt-dev - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 From 2be7e99ba9782a7e05c8d07a10ce60af2a207e68 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 20 Nov 2025 01:32:12 +0100 Subject: [PATCH 055/136] CI updated --- .github/workflows/rolling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index e65e65c5..10f867d5 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -25,7 +25,7 @@ jobs: required-ros-distributions: rolling - name: Install nlopt - run: sudo apt install -y libnlopt-dev + run: sudo apt install -y libnlopt* - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 From 661501f9633f5cd55020b65af269002688d5602e Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 20 Nov 2025 01:52:43 +0100 Subject: [PATCH 056/136] CI updated --- controllers/easynav_mpc_controller/CMakeLists.txt | 6 +++--- .../include/easynav_mpc_controller/MPCController.hpp | 6 ++++-- .../src/easynav_mpc_controller/MPCController.cpp | 12 ++++++------ 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index 74ed5902..6d021ce2 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -18,16 +18,16 @@ find_package(tf2_ros REQUIRED) find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED NO_MODULE) find_package(NLopt QUIET) -if (NOT NLopt_FOUND) +if(NOT NLopt_FOUND) find_path(NLOPT_INCLUDE_DIR nlopt.hpp) find_library(NLOPT_LIBRARY nlopt) - if (NLOPT_INCLUDE_DIR AND NLOPT_LIBRARY) + 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) +if(NOT NLopt_FOUND) message(FATAL_ERROR "NLopt not found. Install libnlopt-dev or set NLopt_DIR/CMAKE_PREFIX_PATH.") endif() diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 14eaa9e8..823a5a7d 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -64,7 +64,9 @@ class MPCController : public ControllerMethodBase /// \brief Publishes the selected path by MPC controller /// \param pose Eigen::Vector3d of the robot /// \param best_path Path generated by MPC - void publish_mpc_path(const Eigen::Vector3d & position, const Eigen::Vector3d & orientation, const std::vector & best_path); + void publish_mpc_path( + const Eigen::Vector3d & position, const Eigen::Vector3d & orientation, + const std::vector & best_path); protected: int horizon_steps_{5}; ///< Prediction horizon for MPC. @@ -76,7 +78,7 @@ class MPCController : public ControllerMethodBase rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path markers. private: - Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost + Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost double qtheta_ {3.0}; diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 953270b3..2b15bf81 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -121,8 +121,10 @@ MPCController::on_initialize() return {}; } -void -MPCController::publish_mpc_path(const Eigen::Vector3d & position, const Eigen::Vector3d & orientation, const std::vector & best_vel) +void +MPCController::publish_mpc_path( + const Eigen::Vector3d & position, + const Eigen::Vector3d & orientation, const std::vector & best_vel) { visualization_msgs::msg::MarkerArray path; visualization_msgs::msg::Marker points; @@ -139,8 +141,7 @@ MPCController::publish_mpc_path(const Eigen::Vector3d & position, const Eigen::V points.color.a = 0.8; Eigen::Vector3d state; - for (size_t i = 0; i + 1 < best_vel.size(); i += 2) - { + for (size_t i = 0; i + 1 < best_vel.size(); i += 2) { double v = best_vel[i]; double w = best_vel[i + 1]; geometry_msgs::msg::Point p; @@ -233,8 +234,7 @@ MPCController::update_rt(NavState & nav_state) try { nlopt::result result = opt.optimize(u, minf); if(verbose_) { - if (result != nlopt::SUCCESS) - { + if (result != nlopt::SUCCESS) { std::cerr << "Optimization Stopped " << std::endl; std::cout << "Result: " << result << std::endl; } else { From cff44bb09697573fd752511e192b6dbbc0badce5 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Fri, 21 Nov 2025 18:35:32 +0100 Subject: [PATCH 057/136] Cleanup unused headers --- .gitignore | 8 + .../easynav_costmap_common/costmap_2d.hpp | 6 +- .../easynav_costmap_common/geometry_utils.hpp | 1 - .../occ_grid_values.hpp | 1 + .../src/easynav_costmap_common/costmap_2d.cpp | 4 - .../tests/costmap_2d_tests.cpp | 1 - .../easynav_simple_common/SimpleMap.hpp | 4 - .../tests/simple_maps_tests.cpp | 2 - .../MPPIController.hpp | 5 - .../MPPIController.cpp | 3 - .../easynav_mppi_controller/MPPIOptimizer.cpp | 4 - .../SerestController.hpp | 4 +- .../SerestController.cpp | 3 +- .../SimpleController.hpp | 1 - .../easynav_vff_controller/VffController.hpp | 1 - .../easynav_vff_controller/VffController.cpp | 1 - .../AMCLLocalizer.hpp | 5 - .../AMCLLocalizer.cpp | 1 - .../easynav_gps_localizer/GpsLocalizer.hpp | 3 - .../AMCLLocalizer.hpp | 6 +- .../AMCLLocalizer.cpp | 2 - .../AMCLLocalizer.hpp | 5 - .../AMCLLocalizer.cpp | 1 - .../BonxaiMapsManager.hpp | 15 -- .../easynav_bonxai_maps_manager/utils.hpp | 34 --- .../BonxaiMapsManager.cpp | 4 - .../src/easynav_bonxai_maps_manager/utils.cpp | 22 -- .../CostmapMapsManager.hpp | 10 - .../filters/CostmapFilter.hpp | 1 - .../filters/InflationFilter.hpp | 5 - .../filters/ObstacleFilter.hpp | 3 - .../CostmapMapsManager.cpp | 3 - .../filters/CostmapFilter.cpp | 3 - .../filters/InflationFilter.cpp | 2 - .../filters/ObstacleFilter.cpp | 1 - .../tests/costmap_mapsmanager_tests.cpp | 12 +- .../NavMapMapsManager.hpp | 10 - .../filters/InflationFilter.hpp | 7 +- .../filters/NavMapFilter.hpp | 3 +- .../filters/ObstacleFilter.hpp | 6 +- .../easynav_navmap_maps_manager/utils.hpp | 70 ------ .../NavMapMapsManager.cpp | 2 - .../filters/InflationFilter.cpp | 3 +- .../filters/NavMapFilter.cpp | 3 - .../filters/ObstacleFilter.cpp | 1 - .../src/easynav_navmap_maps_manager/utils.cpp | 204 ----------------- .../tests/navmap_mapsmanager_tests.cpp | 9 +- .../tests/test_utils.cpp | 205 ------------------ .../OctomapMapsManager.hpp | 10 - .../filters/OctomapFilter.hpp | 2 +- .../easynav_octomap_maps_manager/utils.hpp | 37 ---- .../OctomapMapsManager.cpp | 2 - .../filters/OctomapFilter.cpp | 1 - .../easynav_octomap_maps_manager/utils.cpp | 25 --- .../SimpleMapsManager.hpp | 12 - .../SimpleMapsManager.cpp | 1 - .../tests/simple_mapsmanager_tests.cpp | 2 +- .../CostmapPlanner.hpp | 1 - .../CostmapPlanner.cpp | 1 - .../easynav_navmap_planner/AStarPlanner.hpp | 1 - .../easynav_navmap_planner/AStarPlanner.cpp | 3 - .../easynav_simple_planner/SimplePlanner.hpp | 1 - .../easynav_simple_planner/SimplePlanner.cpp | 1 - 63 files changed, 24 insertions(+), 786 deletions(-) create mode 100644 .gitignore delete mode 100644 maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/utils.hpp delete mode 100644 maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/utils.cpp delete mode 100644 maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/utils.hpp delete mode 100644 maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/utils.cpp delete mode 100644 maps_managers/easynav_navmap_maps_manager/tests/test_utils.cpp delete mode 100644 maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/utils.hpp delete mode 100644 maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/utils.cpp 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/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..3779bec6 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,10 @@ #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 "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/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index d3f97cb6..62a833b8 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 { 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..6f57ce38 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp @@ -11,19 +11,14 @@ #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 { 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..54c07378 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -22,10 +22,7 @@ #include -#include "tf2/utils.hpp" - #include "easynav_mppi_controller/MPPIController.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "nav_msgs/msg/odometry.hpp" 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..a7d86035 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp @@ -4,10 +4,6 @@ #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/include/easynav_serest_controller/SerestController.hpp b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp index 9a22d258..0cbe7a4c 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -23,13 +23,11 @@ #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" 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..aa3a813b 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -24,7 +24,8 @@ #include #include -#include "easynav_common/types/Perceptions.hpp" +#include "tf2/utils.hpp" + #include "easynav_common/types/PointPerception.hpp" #include "easynav_serest_controller/SerestController.hpp" 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..2dfd5c7d 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp @@ -11,7 +11,6 @@ #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" 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..980549b5 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -28,7 +28,6 @@ #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" 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 f3f6b270..febf49f3 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -28,7 +28,6 @@ #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/goals.hpp" -#include "nav_msgs/msg/path.hpp" #include #include 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 6381a32f..1e763825 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 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 c653b573..ddeb3be2 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -27,7 +27,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_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" 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..fc731c9d 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -28,11 +28,8 @@ #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 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..843e76c6 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" 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 afd13218..5391b3da 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include #include @@ -40,7 +39,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" 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..671c11fc 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 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..79357fd3 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -27,7 +27,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_simple_common/SimpleMap.hpp" 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..fd2ba8cd 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 { 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..ba54589c 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 @@ -22,9 +22,6 @@ #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 +31,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" 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/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp index 3499a0e6..367b482f 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 { 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..23934c22 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 @@ -24,7 +24,6 @@ #include #include -#include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" 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..f9e4aff3 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 @@ -44,12 +44,7 @@ #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" 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..7195ad0e 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 @@ -24,9 +24,6 @@ #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" 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..da8ea2c1 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 @@ -22,12 +22,9 @@ #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 "ament_index_cpp/get_package_share_directory.hpp" 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..785bc235 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 @@ -21,9 +21,6 @@ #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 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..3fc2d43e 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 @@ -44,8 +44,6 @@ #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" 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..a6b2b88d 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 @@ -23,7 +23,6 @@ #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" 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..cb338cf0 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 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..13f707a4 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 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..fa3c6263 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 @@ -24,9 +24,6 @@ #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 std::expected 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 6733a9d0..389992fa 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 @@ -24,8 +24,7 @@ #include #include -#include "navmap_core/NavMap.hpp" - +#include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav 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 38833490..bd07c70e 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 @@ -24,8 +24,6 @@ #include #include -#include "pluginlib/class_loader.hpp" - #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -41,8 +39,8 @@ class ObstacleFilter : public NavMapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); - virtual void update(::easynav::NavState & nav_state); + virtual std::expected on_initialize() override; + virtual void update(::easynav::NavState & nav_state) override; virtual bool is_adding_layer() override {return true;} virtual std::string get_layer_name() override {return "obstacles";} 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 409beaf0..ae231de8 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 @@ -22,8 +22,6 @@ #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 "navmap_core/NavMap.hpp" 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 e79104e1..f0be22fe 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 @@ -41,10 +41,9 @@ #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" 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..78608d0d 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 @@ -21,9 +21,6 @@ #include #include -#include "navmap_core/NavMap.hpp" -#include "easynav_common/types/NavState.hpp" - #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" 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 3f4a29c6..9eaba396 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 @@ -23,7 +23,6 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/Perceptions.hpp" #include "easynav_common/types/PointPerception.hpp" #include "navmap_core/NavMap.hpp" 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..e1ede33f 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 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/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp index 4654a14d..ec358d5d 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 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..abe0948b 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 @@ -25,7 +25,7 @@ #include #include "octomap/octomap.h" - +#include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav 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/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp index 8404d664..cc3a05b0 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 @@ -22,8 +22,6 @@ #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" 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..2e03426a 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 @@ -22,7 +22,6 @@ #include #include "octomap/octomap.h" -#include "easynav_common/types/NavState.hpp" #include "easynav_octomap_maps_manager/filters/OctomapFilter.hpp" 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_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..a7455b63 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 { 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..7292bced 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 @@ -23,7 +23,6 @@ #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" 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 8da903ba..17604345 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" 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..9df455ef 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" 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..2a87c50f 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include "easynav_costmap_planner/CostmapPlanner.hpp" 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..cfa40312 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -23,7 +23,6 @@ #ifndef EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ #define EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ -#include #include #include "nav_msgs/msg/path.hpp" 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 758c7277..476a3d4c 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -21,11 +21,8 @@ /// \brief Implementation of the AStarPlanner class using A* on ::navmap::NavMap (triangle graph). #include -#include -#include #include #include -#include #include #include 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..f54b9124 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" 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..76b76179 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include "easynav_simple_planner/SimplePlanner.hpp" From 7d1dbf2ae3ed599d277a55fa6c8e5a038dc69fc6 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 21 Nov 2025 19:39:00 +0100 Subject: [PATCH 058/136] Dependencies were fixed --- .github/workflows/rolling.yaml | 4 ---- .../include/easynav_mpc_controller/MPCController.hpp | 5 +---- controllers/easynav_mpc_controller/package.xml | 4 +++- .../src/easynav_mpc_controller/MPCController.cpp | 6 +++--- 4 files changed, 7 insertions(+), 12 deletions(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 10f867d5..788d0c04 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -23,10 +23,6 @@ jobs: uses: ros-tooling/setup-ros@0.7.15 with: required-ros-distributions: rolling - - - name: Install nlopt - run: sudo apt install -y libnlopt* - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 823a5a7d..24d9f93f 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -7,9 +7,6 @@ #ifndef EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ #define EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ -#include -#include -#include #include #include #include @@ -79,7 +76,7 @@ class MPCController : public ControllerMethodBase private: Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost - Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost + Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost double qtheta_ {3.0}; geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 34c3ca91..52ebb254 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -8,7 +8,9 @@ GPL-3.0-only ament_cmake - nlopt + libnlopt-cxx-dev + libnlopt-dev + libnlopt0 easynav_common easynav_core diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 2b15bf81..72dc17ba 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -234,11 +234,11 @@ MPCController::update_rt(NavState & nav_state) try { nlopt::result result = opt.optimize(u, minf); if(verbose_) { - if (result != nlopt::SUCCESS) { - std::cerr << "Optimization Stopped " << std::endl; + if (result > 0) { + std::cerr << "Optimization Successful " << std::endl; std::cout << "Result: " << result << std::endl; } else { - std::cerr << "Optimization Successful " << std::endl; + std::cerr << "Optimization Unsuccessful " << std::endl; } } From 7c82efe44b37a44be47f0516c144aecd5713f653 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 21 Nov 2025 20:10:51 +0100 Subject: [PATCH 059/136] Dependencies were fixed --- .../easynav_navmap_maps_manager/filters/InflationFilter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..ae10b591 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,7 +21,7 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ -#include +#include #include From 1aee136bd67a6625de90d7ac485dc36217a6aae6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 22 Nov 2025 18:43:25 +0100 Subject: [PATCH 060/136] Minor improvement MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../AMCLLocalizer.cpp | 163 ++++++++++++++---- 1 file changed, 125 insertions(+), 38 deletions(-) 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 afd13218..a83a2e96 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -352,6 +352,7 @@ struct CoordEq // ---------- AMCLLocalizer ---------- AMCLLocalizer::AMCLLocalizer() +: rng_(std::random_device{}()) { NavState::register_printer( [](const nav_msgs::msg::Odometry & odom) { @@ -525,6 +526,7 @@ void AMCLLocalizer::update(NavState & nav_state) void AMCLLocalizer::predict(NavState & nav_state) { + auto t0 = get_node()->now(); if (!initialized_odom_) {if (compute_odom_from_tf_) {update_odom_from_tf();} return;} if (compute_odom_from_tf_) {update_odom_from_tf();} @@ -541,17 +543,18 @@ 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_); + + auto t1 = get_node()->now(); 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,10 +569,12 @@ void AMCLLocalizer::predict(NavState & nav_state) std::size_t sidx = 0; ::navmap::NavCelId cid = std::numeric_limits::max(); Eigen::Vector3f bary, hit_eig; + auto ta = get_node()->now(); 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); + auto tb = get_node()->now(); if (ok) { tf2::Vector3 hit = to_tf(hit_eig); @@ -594,12 +599,21 @@ void AMCLLocalizer::predict(NavState & nav_state) } } + auto t2 = get_node()->now(); + last_odom_ = odom_; pose_ = getEstimatedPose(); tf2::Transform map2bf = pose_; tf2::Transform map2odom = map2bf * odom_.inverse(); publishTF(map2odom); publishEstimatedPose(map2bf); + + auto t3 = get_node()->now(); + + std::cerr << "t1 " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; + std::cerr << "t2 " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; + std::cerr << "t2* " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() / particles_.size() << std::endl; + std::cerr << "t3 " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; } // ---------- perception scoring helpers ---------- @@ -820,13 +834,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); @@ -991,34 +998,89 @@ 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()); geometry_msgs::msg::PoseWithCovarianceStamped msg; msg.header.stamp = get_node()->now(); msg.header.frame_id = get_tf_prefix() + "map"; + msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); msg.pose.pose.position.z = mean.z(); @@ -1029,11 +1091,14 @@ 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(); @@ -1042,19 +1107,40 @@ nav_msgs::msg::Odometry AMCLLocalizer::get_pose() 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]; @@ -1069,6 +1155,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; } From 246081c3c1653a89bc836533e4322348cfede4b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 22 Nov 2025 19:07:41 +0100 Subject: [PATCH 061/136] linting MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../AMCLLocalizer.cpp | 20 ------------------- 1 file changed, 20 deletions(-) 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 82edff62..a3328914 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -524,7 +524,6 @@ void AMCLLocalizer::update(NavState & nav_state) void AMCLLocalizer::predict(NavState & nav_state) { - auto t0 = get_node()->now(); if (!initialized_odom_) {if (compute_odom_from_tf_) {update_odom_from_tf();} return;} if (compute_odom_from_tf_) {update_odom_from_tf();} @@ -549,8 +548,6 @@ void AMCLLocalizer::predict(NavState & nav_state) double noisy_y = yaw + n_yaw(rng_); - auto t1 = get_node()->now(); - for (auto & p : particles_) { 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); @@ -597,21 +594,12 @@ void AMCLLocalizer::predict(NavState & nav_state) } } - auto t2 = get_node()->now(); - last_odom_ = odom_; pose_ = getEstimatedPose(); tf2::Transform map2bf = pose_; tf2::Transform map2odom = map2bf * odom_.inverse(); publishTF(map2odom); publishEstimatedPose(map2bf); - - auto t3 = get_node()->now(); - - std::cerr << "t1 " << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; - std::cerr << "t2 " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; - std::cerr << "t2* " << std::fixed << std::setprecision(10) << (t2 - t1).seconds() / particles_.size() << std::endl; - std::cerr << "t3 " << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; } // ---------- perception scoring helpers ---------- @@ -735,8 +723,6 @@ void AMCLLocalizer::correct(NavState & nav_state) // 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) { @@ -768,8 +754,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_) { @@ -788,8 +772,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) { @@ -810,8 +792,6 @@ void AMCLLocalizer::correct(NavState & nav_state) p.weight /= total_w; } } - - auto t4 = get_node()->now(); } // ------------------- reseed ------------------- From fb65fd46deb7f1305851eacc907c921fe8c39a65 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 24 Nov 2025 08:07:22 +0100 Subject: [PATCH 062/136] Optimize execution MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../MPPIController.cpp | 6 +- .../SerestController.cpp | 2 +- .../easynav_simple_controller/CMakeLists.txt | 6 + .../easynav_simple_controller/package.xml | 2 + .../SimpleController.cpp | 5 +- .../easynav_vff_controller/VffController.cpp | 2 +- .../AMCLLocalizer.cpp | 2 +- .../AMCLLocalizer.hpp | 1 - .../AMCLLocalizer.cpp | 18 +- .../AMCLLocalizer.cpp | 2 +- .../NavMapMapsManager.cpp | 6 +- .../filters/InflationFilter.cpp | 6 +- .../CostmapPlanner.cpp | 4 +- .../easynav_navmap_planner/AStarPlanner.hpp | 48 +++-- .../easynav_navmap_planner/AStarPlanner.cpp | 202 +++++++++++------- .../easynav_simple_planner/SimplePlanner.cpp | 4 +- 16 files changed, 196 insertions(+), 120 deletions(-) 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 54c07378..783e208f 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -144,7 +144,7 @@ MPPIController::update_rt(NavState & nav_state) 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 @@ -164,8 +164,8 @@ 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 & filtered = PointPerceptionsOpsView(perceptions) .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) 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 aa3a813b..717c9cc5 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -301,7 +301,7 @@ SerestController::closest_obstacle_distance( // 2) Analizar los sensores de distancia if (!nav_state.has("points")) {return std::numeric_limits::infinity();} - const auto perceptions = nav_state.get("points"); + const auto & perceptions = nav_state.get("points"); auto fused = PointPerceptionsOpsView(perceptions) .downsample(0.3) .fuse(get_tf_prefix() + "base_link") diff --git a/controllers/easynav_simple_controller/CMakeLists.txt b/controllers/easynav_simple_controller/CMakeLists.txt index c3346d89..d3542acb 100644 --- a/controllers/easynav_simple_controller/CMakeLists.txt +++ b/controllers/easynav_simple_controller/CMakeLists.txt @@ -15,6 +15,8 @@ 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 +32,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 +74,8 @@ ament_export_dependencies( easynav_core pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs ) 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..101bba50 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -23,6 +23,7 @@ #include #include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "easynav_simple_controller/SimpleController.hpp" @@ -100,7 +101,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 +116,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_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index febf49f3..ec916061 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -257,7 +257,7 @@ 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"); auto fused = PointPerceptionsOpsView(perceptions) 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 ddeb3be2..c34e6b8f 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -570,7 +570,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"); 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 843e76c6..59629c37 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -234,7 +234,6 @@ class AMCLLocalizer : public LocalizerMethodBase * @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 a3328914..da00a8ce 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -496,7 +496,7 @@ void AMCLLocalizer::update_odom_from_tf() 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, @@ -529,7 +529,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); @@ -565,7 +567,7 @@ void AMCLLocalizer::predict(NavState & nav_state) Eigen::Vector3f bary, hit_eig; auto ta = get_node()->now(); - 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())), sidx, cid, bary, &hit_eig, opts); @@ -577,7 +579,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) { @@ -692,7 +694,7 @@ void AMCLLocalizer::correct(NavState & nav_state) 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 +703,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,8 +719,8 @@ 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_); 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 79357fd3..ea441228 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -377,7 +377,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"); 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 ae231de8..6ba9390e 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 @@ -226,10 +226,10 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) filter->set_map_resolution(resolution_); filter->update(nav_state); - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + 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()); + 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 f0be22fe..2d9e8f7d 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 @@ -227,10 +227,10 @@ void InflationFilter::update(::easynav::NavState & nav_state) return; } - auto nm = nav_state.get<::navmap::NavMap>("map.navmap"); + navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); const bool ok = inflate_layer_u8( - nm, + navmap_, "obstacles", "inflated_obstacles", inflation_radius_, @@ -242,7 +242,7 @@ void InflationFilter::update(::easynav::NavState & nav_state) return; } - nav_state.set("map.navmap", nm); + nav_state.set("map.navmap", navmap_); } } // namespace navmap 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 2a87c50f..0d05e379 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -108,14 +108,14 @@ void CostmapPlanner::update(NavState & nav_state) 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; if (goals.header.frame_id != get_tf_prefix() + "map") { 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 cfa40312..c5fcb482 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -24,6 +24,8 @@ #define EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ #include +#include +#include #include "nav_msgs/msg/path.hpp" @@ -38,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 { @@ -54,7 +56,7 @@ 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. */ @@ -71,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. * @@ -113,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 476a3d4c..a498f354 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "easynav_navmap_planner/AStarPlanner.hpp" @@ -67,11 +68,9 @@ std::expected AStarPlanner::on_initialize() 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( @@ -86,16 +85,15 @@ 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; if (goals.header.frame_id != get_tf_prefix() + "map") { @@ -113,7 +111,6 @@ void AStarPlanner::update(NavState & nav_state) } current_goal_ = 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; @@ -126,7 +123,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_); @@ -135,7 +132,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, @@ -160,9 +156,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 @@ -198,7 +195,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]); @@ -207,8 +205,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]); }; @@ -263,7 +261,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); } @@ -285,6 +283,42 @@ static inline bool layer_exists(const ::navmap::NavMap & nm, const std::string & 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, @@ -296,146 +330,162 @@ std::vector AStarPlanner::a_star_path( if (nm.navcels.empty()) {return {};} // 1) Locate start and goal NavCels (fallback to closest triangle if necessary) - size_t sidx_s = 0, sidx_g = 0; + 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(); - // 2) Precompute centroids (used for cost, heuristic, and edge lengths) - 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"; + + // 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 = C[a] - C[b]; + const auto d = centroids_[a] - centroids_[b]; return static_cast(d.norm()); }; - // 3) Choose cost layer: prefer "inflated_obstacles", fallback to "obstacles" - const std::string cost_layer = - layer_exists(nm, "inflated_obstacles") ? "inflated_obstacles" : "obstacles"; - - // Cache per-NavCel uint8_t cost values (0..255) - std::vector occ(N, FREE_SPACE); - for (NavCelId c = 0; c < N; ++c) { - // If the cell has no stored value, assume FREE_SPACE (0) - occ[c] = nm.layer_get(cost_layer, c, FREE_SPACE); + // 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 + // Traversability: block lethal and unknown. auto traversable = [&](NavCelId c) -> bool { - const std::uint8_t v = occ[c]; + 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 + // 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]; + 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 + // 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 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)) + // 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 base = euclid(from, to); - if (!std::isfinite(base) || base <= 0.0) {return std::numeric_limits::infinity();} + 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();} + 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. + // 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* 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;} }; + // 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 parent(N, std::numeric_limits::max()); - std::vector in_open(N, 0); auto h = [&](NavCelId a, NavCelId b) -> double { - // Heuristic: pure Euclidean distance → admissible (never overestimates) + // Heuristic: pure Euclidean distance → admissible (never overestimates). return euclid(a, b); }; - g[cid_start] = 0.0; + 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;} - for (NavCelId v : nm.navcel_neighbors(u)) { - const size_t vidx = static_cast(v); - if (vidx >= N) {continue;} + 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; + } - // Skip non-traversable neighbors (lethal or unknown) + // 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) Path reconstruction (centroid-based 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;} 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 76b76179..4a902ea0 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -110,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_); @@ -130,7 +130,7 @@ 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; auto downsampled_map = map_typed.downsample(0.2); From 0c99f593f9d17c9efa179fe3fcea79ba06a61c36 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 24 Nov 2025 08:13:26 +0100 Subject: [PATCH 063/136] Fix build MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_navmap_planner/AStarPlanner.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 a498f354..d9dbf2fc 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -97,8 +97,9 @@ void AStarPlanner::update(NavState & nav_state) const auto & goal = goals.goals.front().pose; 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()); + RCLCPP_WARN( + get_node()->get_logger(), "Goals frame is not 'map': %s", + goals.header.frame_id.c_str()); return; } @@ -111,6 +112,7 @@ void AStarPlanner::update(NavState & nav_state) } current_goal_ = 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; From 7e91ed9db98e357bbd0676c88b9f76d2b3e3dacc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 24 Nov 2025 10:54:23 +0100 Subject: [PATCH 064/136] Optimize random genearator MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_localizer/AMCLLocalizer.hpp | 2 +- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 11 +++++------ .../easynav_navmap_localizer/AMCLLocalizer.hpp | 2 +- 3 files changed, 7 insertions(+), 8 deletions(-) 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 1e763825..0c4d117d 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -187,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_; 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 c34e6b8f..cf95fa59 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -161,6 +161,7 @@ using namespace std::chrono_literals; AMCLLocalizer::AMCLLocalizer() +: rng_(std::random_device{}()) { NavState::register_printer( [](const nav_msgs::msg::Odometry & odom) { @@ -527,8 +528,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_); @@ -540,11 +539,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); 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 59629c37..6cbeab6b 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -184,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_; From 4e7065f75efbd24af9df89c63d0a46ea670bb552 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 24 Nov 2025 11:07:15 +0100 Subject: [PATCH 065/136] Fix Link error in controller MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_serest_controller/CMakeLists.txt | 6 ++++++ controllers/easynav_serest_controller/package.xml | 2 ++ .../src/easynav_serest_controller/SerestController.cpp | 1 + 3 files changed, 9 insertions(+) diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index 26541250..0067ae1f 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/controllers/easynav_serest_controller/CMakeLists.txt @@ -15,6 +15,8 @@ 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 +31,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 +72,8 @@ ament_export_dependencies( easynav_core pluginlib tf2_ros + tf2 + tf2_geometry_msgs geometry_msgs nav_msgs ) 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 717c9cc5..fbe6de4a 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -25,6 +25,7 @@ #include #include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "easynav_common/types/PointPerception.hpp" From 84ef8dfc06c4b374001189a71e5199f380f90911 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 25 Nov 2025 09:26:19 +0100 Subject: [PATCH 066/136] Update perception bound MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CostmapMapsManager.hpp | 5 + .../filters/CostmapFilter.hpp | 9 +- .../filters/InflationFilter.hpp | 23 +++- .../CostmapMapsManager.cpp | 19 ++- .../filters/InflationFilter.cpp | 120 ++++++++++++++++-- .../filters/ObstacleFilter.cpp | 18 ++- 6 files changed, 172 insertions(+), 22 deletions(-) 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 367b482f..49687461 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 @@ -96,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 23934c22..4f2dfef6 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 @@ -31,6 +31,12 @@ namespace easynav { +struct ObstacleBounds +{ + double min_x, min_y, max_x, max_y; +}; + + class CostmapFilter { public: @@ -45,10 +51,11 @@ class CostmapFilter virtual std::expected on_initialize() = 0; virtual void update(NavState & nav_state) = 0; + const std::string & get_plugin_name() const; + protected: std::shared_ptr get_node() const; - const std::string & get_plugin_name() const; const std::string & get_tf_prefix() const; 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 f9e4aff3..60e15040 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 @@ -197,7 +197,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, @@ -224,6 +230,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/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index da8ea2c1..3c409d56 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 @@ -187,17 +187,26 @@ 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_); for (const auto & filter : costmap_filters_) { + auto t0 = get_node()->now(); filter->update(nav_state); + auto t1 = get_node()->now(); + + std::cerr << "[" << filter->get_plugin_name() << "] " << + std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; } - 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_); - final_dynamic_map.toOccupancyGridMsg(dynamic_grid_msg_); + 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_occ_pub_->publish(dynamic_grid_msg_); 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 3fc2d43e..173eaa53 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 @@ -99,19 +99,84 @@ InflationFilter::on_initialize() void InflationFilter::update(NavState & nav_state) { - Costmap2D dynamic_map = nav_state.get("map.dynamic.filtered"); + auto t0 = get_node()->now(); + 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)) { + std::cerr << "Recomputado!!!" << std::endl; + recompute_static_inflation_(static_map); + } + + auto t1 = get_node()->now(); if (!matchedSize_) { cell_inflation_radius_ = cellDistance(dynamic_map, inflation_radius_); matchSize(dynamic_map); 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); -} + auto t2 = get_node()->now(); + + 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); + } + } + + auto t3 = get_node()->now(); + + std::cerr << "[" << min_i << " - " << max_i << "] [" << min_j << " - " << max_j << "]" << std::endl; + + updateCosts(dynamic_map, min_i, min_j, max_i, max_j); + + auto t4 = get_node()->now(); + + + 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); + } + } + auto t5 = get_node()->now(); + + std::cerr << "t1" << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; + std::cerr << "t2" << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; + std::cerr << "t3" << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; + std::cerr << "t4" << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; + std::cerr << "t5" << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; + + nav_state.set("map.dynamic.filtered", dynamic_map_ptr); +} void InflationFilter::matchSize(easynav::Costmap2D & costmap) @@ -282,15 +347,6 @@ InflationFilter::updateCosts( } } -/** - * @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, @@ -389,6 +445,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 a6b2b88d..0f089294 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 @@ -54,7 +54,8 @@ 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; auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map.getResolution()) @@ -62,14 +63,27 @@ ObstacleFilter::update(NavState & nav_state) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); + 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 From 3338c81ca7266d43c9526ac527ab93b8a603720b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 25 Nov 2025 09:43:06 +0100 Subject: [PATCH 067/136] First UpdateCosts optimization MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/InflationFilter.cpp | 144 ++++++++++-------- 1 file changed, 77 insertions(+), 67 deletions(-) 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 173eaa53..3b30d3ba 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 @@ -219,107 +219,120 @@ 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(); + 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; - 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); + // Ensure seen_ matches full map size, but do NOT clear it yet + if (seen_.size() != cell_count) { + seen_.assign(cell_count, false); } - std::fill(begin(seen_), end(seen_), false); + // Reuse all inflation distance bins + 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 radius expansion) 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 window by inflation radius (in cells) 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 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_OBSTACLE or NO_INFORMATION if configured) + // Also reset visited flags (seen_) only inside the expanded window. + // -------------------------------------------------------------------------- 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); + + // Reset visited flag only for cells inside the relevant region + seen_[index] = false; + + 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 obstacles → nothing to inflate + if (obs_bin.empty()) { + return; + } + + // -------------------------------------------------------------------------- + // 2) BFS over inflation distance bins + // -------------------------------------------------------------------------- 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); + + // Skip if already processed 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 from obstacle + const unsigned char cost = costLookup(mx, my, sx, sy); + const unsigned char old_cost = master_array[index]; + + // Apply inflation only in the original 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) + 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 { @@ -327,23 +340,20 @@ 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); + // Push 4-connected neighbors into their corresponding bins + 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(); } } From eeab912ff74f372e4b9ed7690ee24a1a116db4bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 25 Nov 2025 09:51:46 +0100 Subject: [PATCH 068/136] Final optimization MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/InflationFilter.hpp | 4 +- .../filters/InflationFilter.cpp | 110 +++++++++--------- 2 files changed, 54 insertions(+), 60 deletions(-) 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 60e15040..23dd3476 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 @@ -230,10 +230,10 @@ 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}; 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 3b30d3ba..99f58032 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 @@ -99,19 +99,15 @@ InflationFilter::on_initialize() void InflationFilter::update(NavState & nav_state) { - auto t0 = get_node()->now(); - 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)) { - std::cerr << "Recomputado!!!" << std::endl; recompute_static_inflation_(static_map); } - auto t1 = get_node()->now(); if (!matchedSize_) { cell_inflation_radius_ = cellDistance(dynamic_map, inflation_radius_); matchSize(dynamic_map); @@ -125,14 +121,12 @@ InflationFilter::update(NavState & nav_state) int max_i = size_x; int max_j = size_y; - auto t2 = get_node()->now(); - 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)) + 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); @@ -149,15 +143,8 @@ InflationFilter::update(NavState & nav_state) } } - auto t3 = get_node()->now(); - - std::cerr << "[" << min_i << " - " << max_i << "] [" << min_j << " - " << max_j << "]" << std::endl; - updateCosts(dynamic_map, min_i, min_j, max_i, max_j); - auto t4 = get_node()->now(); - - 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)); @@ -167,14 +154,6 @@ InflationFilter::update(NavState & nav_state) } } - auto t5 = get_node()->now(); - - std::cerr << "t1" << std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; - std::cerr << "t2" << std::fixed << std::setprecision(10) << (t2 - t1).seconds() << std::endl; - std::cerr << "t3" << std::fixed << std::setprecision(10) << (t3 - t2).seconds() << std::endl; - std::cerr << "t4" << std::fixed << std::setprecision(10) << (t4 - t3).seconds() << std::endl; - std::cerr << "t5" << std::fixed << std::setprecision(10) << (t5 - t4).seconds() << std::endl; - nav_state.set("map.dynamic.filtered", dynamic_map_ptr); } @@ -239,29 +218,44 @@ InflationFilter::updateCosts( const unsigned int size_y = master_grid.getSizeInCellsY(); const std::size_t cell_count = static_cast(size_x) * size_y; - // Ensure seen_ matches full map size, but do NOT clear it yet - if (seen_.size() != cell_count) { - seen_.assign(cell_count, false); + // -------------------------------------------------------------------------- + // Visited flags: stamp-based approach to avoid clearing a full-sized array. + // Each call increases current_stamp, and a cell is considered "seen" if + // seen_stamp[index] == current_stamp. + // -------------------------------------------------------------------------- + static std::vector seen_stamp; + static uint32_t current_stamp = 1u; + + if (seen_stamp.size() != cell_count) { + seen_stamp.assign(cell_count, 0u); + } + + // Increment stamp; on overflow, reset the whole buffer once. + ++current_stamp; + if (current_stamp == 0u) { + std::fill(seen_stamp.begin(), seen_stamp.end(), 0u); + current_stamp = 1u; } - // Reuse all inflation distance bins + // Reuse all inflation distance bins instead of reallocating them for (auto & dist_bin : inflation_cells_) { dist_bin.clear(); } - // Store the original bounding box (before radius expansion) + // 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 window by inflation radius (in cells) + // 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 to map bounds + // 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); @@ -272,8 +266,8 @@ InflationFilter::updateCosts( } // -------------------------------------------------------------------------- - // 1) Collect seeds (LETHAL_OBSTACLE or NO_INFORMATION if configured) - // Also reset visited flags (seen_) only inside the expanded window. + // 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); @@ -281,26 +275,24 @@ InflationFilter::updateCosts( 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); - - // Reset visited flag only for cells inside the relevant region - seen_[index] = false; - const unsigned char cost = master_array[index]; + if (cost == LETHAL_OBSTACLE || - (inflate_around_unknown_ && cost == NO_INFORMATION)) + (inflate_around_unknown_ && cost == NO_INFORMATION)) { obs_bin.emplace_back(i, j, i, j); } } } - // No obstacles → nothing to inflate + // No seeds in this region → nothing to inflate if (obs_bin.empty()) { return; } // -------------------------------------------------------------------------- - // 2) BFS over inflation distance bins + // 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); @@ -313,26 +305,26 @@ InflationFilter::updateCosts( const unsigned int sy = cell.src_y_; const unsigned int index = master_grid.getIndex(mx, my); - // Skip if already processed in this call - if (seen_[index]) { + // Skip if this cell has already been processed in the current call + if (seen_stamp[index] == current_stamp) { continue; } - seen_[index] = true; + seen_stamp[index] = current_stamp; - // Compute inflation cost based on distance from obstacle + // 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 in the original bounding box + // 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) + 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 { @@ -340,7 +332,7 @@ InflationFilter::updateCosts( } } - // Push 4-connected neighbors into their corresponding bins + // Enqueue 4-connected neighbors (clipped to full map bounds) if (mx > 0u) { enqueue(index - 1u, mx - 1u, my, sx, sy); } @@ -354,6 +346,8 @@ InflationFilter::updateCosts( enqueue(index + size_x, mx, my + 1u, sx, sy); } } + + // No need to shrink dist_bin here; capacity is reused on the next call. } } @@ -463,10 +457,10 @@ InflationFilter::needs_recompute_static_(const Costmap2D & static_map) const } 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) + 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; } @@ -484,11 +478,11 @@ InflationFilter::recompute_static_inflation_(const Costmap2D & static_map) 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_.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(); + static_sig_.origin_x = static_map.getOriginX(); + static_sig_.origin_y = static_map.getOriginY(); has_static_inflated_ = true; } From 8792577804ce363698824808e00ec58ae305b62f Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 25 Nov 2025 10:37:42 +0100 Subject: [PATCH 069/136] fix: Prevent A* planner from traversing through obstacles Signed-off-by: Jose Miguel --- .../filters/InflationFilter.cpp | 7 +++++-- .../src/easynav_costmap_planner/CostmapPlanner.cpp | 9 +++------ 2 files changed, 8 insertions(+), 8 deletions(-) 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 3fc2d43e..6ce58d50 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 @@ -77,16 +77,19 @@ 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(); 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 0d05e379..b6ff91c4 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -189,14 +189,11 @@ 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;} + // Calculate traversal cost for free and lightly inflated cells 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; - } double step_cost = (dx == 0 || dy == 0) ? cost_axial_ : cost_diagonal_; double new_cost = cost_so_far[idx(current.x, current.y)] + traversal_cost * step_cost; From 41ee5b91cb3f4e4e22b560a59bf2f26354c678fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 25 Nov 2025 11:35:29 +0100 Subject: [PATCH 070/136] Improvements MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/InflationFilter.cpp | 37 ++++++++----------- 1 file changed, 15 insertions(+), 22 deletions(-) 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 99f58032..1061625f 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 @@ -217,24 +217,17 @@ InflationFilter::updateCosts( 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; - - // -------------------------------------------------------------------------- - // Visited flags: stamp-based approach to avoid clearing a full-sized array. - // Each call increases current_stamp, and a cell is considered "seen" if - // seen_stamp[index] == current_stamp. - // -------------------------------------------------------------------------- - static std::vector seen_stamp; - static uint32_t current_stamp = 1u; - - if (seen_stamp.size() != cell_count) { - seen_stamp.assign(cell_count, 0u); - } - - // Increment stamp; on overflow, reset the whole buffer once. - ++current_stamp; - if (current_stamp == 0u) { - std::fill(seen_stamp.begin(), seen_stamp.end(), 0u); - current_stamp = 1u; + // 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; + } + } } // Reuse all inflation distance bins instead of reallocating them @@ -304,12 +297,11 @@ InflationFilter::updateCosts( const unsigned int sx = cell.src_x_; const unsigned int sy = cell.src_y_; const unsigned int index = master_grid.getIndex(mx, my); - - // Skip if this cell has already been processed in the current call - if (seen_stamp[index] == current_stamp) { + // Process each cell only once in this call + if (seen_[index]) { continue; } - seen_stamp[index] = current_stamp; + seen_[index] = true; // Compute inflation cost based on distance to the nearest obstacle const unsigned char cost = costLookup(mx, my, sx, sy); @@ -372,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 } } From 0c83602b38540492eba11fc4177e59c5d016069c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 26 Nov 2025 11:16:57 +0100 Subject: [PATCH 071/136] Sync README with code MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_vff_controller/README.md | 31 ++++++++++++-- .../easynav_costmap_maps_manager/README.md | 40 ++++++++++++++----- .../easynav_octomap_maps_manager/README.md | 16 +++----- .../easynav_simple_maps_manager/README.md | 4 +- planners/easynav_navmap_planner/README.md | 4 +- 5 files changed, 68 insertions(+), 27 deletions(-) diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index e6cd3315..b2e54ea7 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -4,7 +4,7 @@ ## 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 @@ -21,9 +21,32 @@ Easy Navigation: VFF Controller package. ## 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.) + +## 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. diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 195f710a..11b3a91f 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -45,17 +45,38 @@ 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 +Each entry in `.filters` defines a sub-namespace `.` with at least the key `plugin`, plus any filter-specific parameters. -#### InflationFilter -| Name | 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. | +#### **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. -#### ObstacleFilter -This filter does not declare any ROS parameters apart from `plugin`. +| 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. | +| **Input Key:** | | | Reads point clouds from `NavState` key `"points"`. | +| **Output Key:** | | | Updates `NavState` key `"map.dynamic.obstacle_bounds"` with obstacle bounding box. | +| **Updates:** | | | Marks cells in the dynamic costmap (`map.dynamic.filtered`) as `LETHAL_OBSTACLE` (254). | + +#### **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. + +| Parameter | Type | Default | Description | +|---|---|---:|---| +| `.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. | +| **Input Keys:** | | | Reads `"map.static"` and `"map.dynamic.filtered"` (`Costmap2D`), and optionally `"map.dynamic.obstacle_bounds"` (`ObstacleBounds`). | +| **Output Key:** | | | Writes inflated result back to `"map.dynamic.filtered"`. | +| **Cost Model:** | | | Uses exponential decay: `cost = exp(-cost_scaling_factor * (distance - inscribed_radius)) * 253` for distances beyond inscribed radius. | **Example Configuration** @@ -100,6 +121,7 @@ maps_manager_node: | `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). | --- diff --git a/maps_managers/easynav_octomap_maps_manager/README.md b/maps_managers/easynav_octomap_maps_manager/README.md index 700ce944..9ea021d4 100644 --- a/maps_managers/easynav_octomap_maps_manager/README.md +++ b/maps_managers/easynav_octomap_maps_manager/README.md @@ -26,14 +26,8 @@ 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. @@ -75,8 +69,8 @@ maps_manager_node: ### 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 | @@ -88,7 +82,7 @@ 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.) | --- diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 8c002e1e..4dad4744 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -27,7 +27,7 @@ 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`. | @@ -55,6 +55,8 @@ At the heart of this stack is the SimpleMap data structure. It represents the en | 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 diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index 164419dc..8e48b74e 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -50,11 +50,11 @@ All parameters are declared under the plugin namespace, i.e. | Name | Type | Default | Description | |---|---|---:|---| -| `.layer` | `string` | `"inflated_obstacles"` | Name of the NavMap layer containing 8-bit costs (`FREE_SPACE=0`, `INSCRIBED_INFLATED_OBSTACLE=253`, `LETHAL_OBSTACLE=254`). | | `.cost_factor` | `double` | `2.0` | Multiplicative weight for geometric distance; values > 1 increase the relative importance of distance. | -| `.inflation_penalty` | `double` | `5.0` | Additional penalty proportional to the normalized cell cost; higher values keep the path farther from inflated areas. | | `.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) ### Publications From 1c4ff1fdfcc6643d4d56efa42f2ef140c32a1cfc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 26 Nov 2025 11:22:39 +0100 Subject: [PATCH 072/136] Split Navstate keys tables in filter plugins MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_maps_manager/README.md | 29 +++++++++++++++---- 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 11b3a91f..3d52c7c4 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -55,12 +55,19 @@ Each entry in `.filters` defines a sub-namespace `.` wit 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. | -| **Input Key:** | | | Reads point clouds from `NavState` key `"points"`. | -| **Output Key:** | | | Updates `NavState` key `"map.dynamic.obstacle_bounds"` with obstacle bounding box. | -| **Updates:** | | | Marks cells in the dynamic costmap (`map.dynamic.filtered`) as `LETHAL_OBSTACLE` (254). | + +**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` @@ -69,14 +76,24 @@ Each entry in `.filters` defines a sub-namespace `.` wit 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_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. | -| **Input Keys:** | | | Reads `"map.static"` and `"map.dynamic.filtered"` (`Costmap2D`), and optionally `"map.dynamic.obstacle_bounds"` (`ObstacleBounds`). | -| **Output Key:** | | | Writes inflated result back to `"map.dynamic.filtered"`. | -| **Cost Model:** | | | Uses exponential decay: `cost = exp(-cost_scaling_factor * (distance - inscribed_radius)) * 253` for distances beyond inscribed radius. | + +**NavState Keys:** + +| 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** From cceec3d6ef11a47246278ae0da64eaa90ba04c52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 26 Nov 2025 15:52:54 +0100 Subject: [PATCH 073/136] Remove traces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_maps_manager/CostmapMapsManager.cpp | 5 ----- 1 file changed, 5 deletions(-) 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 3c409d56..2c9e1cb9 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 @@ -196,12 +196,7 @@ CostmapMapsManager::update(NavState & nav_state) nav_state.set("map.dynamic.filtered", dynamic_map_); for (const auto & filter : costmap_filters_) { - auto t0 = get_node()->now(); filter->update(nav_state); - auto t1 = get_node()->now(); - - std::cerr << "[" << filter->get_plugin_name() << "] " << - std::fixed << std::setprecision(10) << (t1 - t0).seconds() << std::endl; } nav_state.set("map.dynamic", dynamic_map_); From ad3355d60dec726a0e6bdf69c0fb4dbb00d6cc0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 26 Nov 2025 19:33:17 +0100 Subject: [PATCH 074/136] Improve efficiency in A* MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CostmapPlanner.cpp | 74 ++++++++++++++++--- 1 file changed, 64 insertions(+), 10 deletions(-) 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 b6ff91c4..b1b88b4e 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -141,6 +141,35 @@ 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(); + 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()) { current_path_.header.stamp = get_node()->now(); @@ -152,9 +181,22 @@ void CostmapPlanner::update(NavState & nav_state) pose_stamped.pose = pose; current_path_.poses.push_back(pose_stamped); } - if (path_pub_->get_subscription_count() > 0) { - path_pub_->publish(current_path_); + // Publish only when the content changed (size as a cheap proxy) + static size_t last_published_size = 0; + if (current_path_.poses.size() != last_published_size) { + if (path_pub_->get_subscription_count() > 0) { + path_pub_->publish(current_path_); + } + last_published_size = current_path_.poses.size(); + } + // 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_); } @@ -169,11 +211,19 @@ 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 = cost_axial_; + const double diagonal_cost = cost_diagonal_; 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; + + 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()); open.push(GridNode{static_cast(sx), static_cast(sy), 0.0, heuristic(sx, sy, gx, gy)}); cost_so_far[idx(sx, sy)] = 0.0; @@ -193,23 +243,24 @@ std::vector CostmapPlanner::a_star_path( if (cell_cost >= INSCRIBED_INFLATED_OBSTACLE) {continue;} // Calculate traversal cost for free and lightly inflated cells - double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost) / LETHAL_OBSTACLE; + 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}; + 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; @@ -217,7 +268,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()); From 2c451c7c391cd00904ea4319081a3ac2f432e96b Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 26 Nov 2025 20:39:16 +0100 Subject: [PATCH 075/136] Optimizer header file was added --- .../easynav_mpc_controller/CMakeLists.txt | 3 + .../easynav_mpc_controller/MPCController.hpp | 19 +- .../easynav_mpc_controller/MPCOptimizer.hpp | 153 +++----- .../easynav_mpc_controller/MPCController.cpp | 129 ++----- .../easynav_mpc_controller/MPCOptimizer.cpp | 338 ++++-------------- 5 files changed, 159 insertions(+), 483 deletions(-) diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index 6d021ce2..b8e9b365 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -17,6 +17,7 @@ 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) @@ -33,6 +34,7 @@ endif() add_library(${PROJECT_NAME} SHARED src/easynav_mpc_controller/MPCController.cpp + src/easynav_mpc_controller/MPCOptimizer.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ @@ -48,6 +50,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC nlopt ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} + ${PCL_LIBRARIES} ) install( diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 24d9f93f..d34f3de1 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -4,6 +4,7 @@ // 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_ @@ -13,7 +14,7 @@ #include #include -#include + #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -24,19 +25,9 @@ #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" +#include "easynav_common/types/PointPerception.hpp" -struct MPCParameters -{ - Eigen::Vector2d goal; - Eigen::Vector3d x0; - Eigen::Vector3d theta0; - Eigen::Matrix2d Q; - Eigen::Matrix2d R; - Eigen::Matrix2d Rd; - double qtheta; - int N; - double dt; -}; +#include "easynav_mpc_controller/MPCOptimizer.hpp" namespace easynav { @@ -72,6 +63,8 @@ class MPCController : public ControllerMethodBase double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. bool verbose_{false}; ///< Value to debug on terminal + std::unique_ptr optimizer_; ///< MPPI optimizer + rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path markers. private: diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp index ba9ba858..0197e019 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -5,12 +5,8 @@ // See for details. // #pragma once -#ifndef EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ -#define EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ - -#include -#include -#include +#ifndef EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ +#define EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/path.hpp" @@ -20,117 +16,60 @@ namespace easynav { -/// \brief Result structure for MPPI optimization containing control commands and trajectories. -struct MPPIResult +class MPCParameters { - double v; ///< Linear velocity command. - double w; ///< Angular velocity command. - std::vector>> all_trajectories; ///< All sampled trajectories. - std::vector> best_trajectory; ///< Best trajectory found during optimization. -}; +public: -struct TrajectorySample -{ - double v; ///< Linear velocity for this sample. - double w; ///< Angular velocity for this sample. - double cost; ///< Cost associated with this trajectory sample. + MPCParameters(Eigen::Vector2d goal, + Eigen::Vector3d x0, + Eigen::Vector3d theta0, + Eigen::Matrix2d Q, + Eigen::Matrix2d R, + Eigen::Matrix2d Rd, + double qtheta, + int N, + double dt, + const pcl::PointCloud & points); + + ~MPCParameters(); + + Eigen::Vector2d goal; + Eigen::Vector3d x0; + Eigen::Vector3d theta0; + Eigen::Matrix2d Q; + Eigen::Matrix2d R; + Eigen::Matrix2d Rd; + double qtheta; + int N; + double dt; + const pcl::PointCloud & points; }; -class MPPIOptimizer +class MPCOptimizer { public: - /// \brief Constructor for MPPIOptimizer. - /// \param num_samples Number of samples to generate for MPPI. - /// \param horizon_steps Number of steps in the prediction horizon. - /// \param dt Time step for the simulation. - /// \param lambda Temperature parameter for MPPI. - /// \param max_lin_vel Maximum linear velocity in m/s. - /// \param max_ang_vel Maximum angular velocity in rad/s. - /// \param max_lin_acc Maximum linear acceleration in m/s^2. - /// \param max_ang_acc Maximum angular acceleration in rad/s^2. - /// \param fov Field of view in radians for trajectory sampling. - /// \param safety_radius Safety radius for obstacle avoidance in meters. - MPPIOptimizer( - double num_samples, double horizon_steps, double dt, double lambda, - double max_lin_vel = 1.0, double max_ang_vel = 1.0, double max_lin_acc = 1.0, - double max_ang_acc = 1.0, double fov = M_PI / 2.0, double safety_radius = 0.6); - - /// \brief Computes the control commands using MPPI optimization. - /// \param current_pose Current pose of the robot. - /// \param path Planned path to follow. - /// \param points Point cloud of the environment. - /// \return MPPIResult containing the best control commands and trajectories. - MPPIResult compute_control( - const geometry_msgs::msg::Pose & current_pose, - const nav_msgs::msg::Path & path, - const pcl::PointCloud & points); -private: - double num_samples_; ///< Number of samples to generate for MPPI. - double horizon_steps_; ///< Number of steps in the prediction horizon. - double dt_; ///< Time step for the simulation. - double lambda_; ///< Temperature parameter for MPPI. - double max_lin_vel_; ///< Maximum linear velocity in m/s. - double max_ang_vel_; ///< Maximum angular velocity in rad/s. - double max_lin_acc_; ///< Maximum linear acceleration in m/s^2. - double max_ang_acc_; ///< Maximum angular acceleration in rad/s^2. - double fov_; ///< Field of view in radians for trajectory sampling. - double safety_radius_; ///< Safety radius for obstacle avoidance in meters. - double last_v_ = 0.0; ///< Last linear velocity command for smoothing. - double last_w_ = 0.0; ///< Last angular velocity command for smoothing. - - std::default_random_engine rng_; ///< Random number generator for sampling. - std::normal_distribution normal_ = std::normal_distribution(0.0, 0.5); ///< Normal distribution for noise in sampling. - std::normal_distribution v_noise_ = std::normal_distribution(0.0, 0.05); ///< Normal distribution for noise in linear velocity. - std::normal_distribution w_noise_ = std::normal_distribution(0.0, 0.02); ///< Normal distribution for noise in angular velocity. - - /// \brief Computes the cost of a trajectory based on its distance to the path and heading error. - /// \param trajectory The trajectory to evaluate. - /// \param path The planned path to follow. - /// \param v Linear velocity of the trajectory. - /// \param w Angular velocity of the trajectory. - /// \param initial_yaw Initial yaw orientation of the robot. - /// \param points The point cloud of the environment. - /// \return The computed cost of the trajectory. - double compute_cost( - const std::vector> & trajectory, - const nav_msgs::msg::Path & path, - double v, double w, double initial_yaw, - const pcl::PointCloud & points); + MPCOptimizer(); + + /// \brief Destructor. + ~MPCOptimizer(); - /// \brief Simulates a trajectory based on initial position, orientation, and velocities. - /// \param x Initial x position. - /// \param y Initial y position. - /// \param yaw Initial yaw orientation. - /// \param v Linear velocity. - /// \param w Angular velocity. - /// \param path The planned path to follow. - /// \param steps Number of steps to simulate. - /// \return The simulated trajectory. - std::vector> simulate_trajectory( - double x, double y, double yaw, - double v, double w, const nav_msgs::msg::Path & path, int steps); - - /// \brief Computes the heading error between the robot's current orientation and the target point. - /// \param robot_yaw Current yaw orientation of the robot. - /// \param target_x X coordinate of the target point. - /// \param target_y Y coordinate of the target point. - /// \param robot_x X coordinate of the robot's current position. - /// \param robot_y Y coordinate of the robot's current position. - /// \return The heading error in radians. - double heading_error( - double robot_yaw, - double target_x, double target_y, - double robot_x, double robot_y); - - /// \brief Computes the shortest angular distance between two angles. - /// \param from Starting angle in radians. - /// \param to Ending angle in radians. - /// \return The shortest angular distance in radians. - double shortest_angular_distance(double from, double to); + Eigen::Vector3d kinematic_model(const Eigen::Vector3d & x, + const Eigen::Vector3d & q, double v, double w, double dt); + + double cost_function(const std::vector & u, + std::vector & grad, void *data); + + static double nlopt_cost_callback(const std::vector & x, + std::vector & grad, void *data); }; +struct NLoptCallbackData { + MPCOptimizer *optimizer; + MPCParameters *params; + }; + } // namespace easynav -#endif // EASYNAV_MPPI_CONTROLLER__MPPIOPTIMIZER_HPP_ +#endif // EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 72dc17ba..c35d45b7 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -22,74 +22,6 @@ #include "easynav_mpc_controller/MPCController.hpp" -Eigen::Vector3d -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 -cost_function(const std::vector & u, 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->N; - double dt = params->dt; - double qtheta = params->qtheta; - double cost = 0.0; - - Eigen::Matrix2d R = params->R; - Eigen::Matrix2d Q = params->Q; - Eigen::Matrix2d Rd = params->Rd; - - 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 = 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; - -} - -static double nlopt_cost_callback( - const std::vector & x, - std::vector & grad, - void *data) -{ - return cost_function(x, grad, data); -} - namespace easynav { @@ -115,6 +47,8 @@ MPCController::on_initialize() node->get_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); node->get_parameter(plugin_name + ".verbose", verbose_); + optimizer_ = std::make_unique(); + mpc_path_pub_ = node->create_publisher("/mpc/path", 10); @@ -145,7 +79,7 @@ MPCController::publish_mpc_path( double v = best_vel[i]; double w = best_vel[i + 1]; geometry_msgs::msg::Point p; - state = kinematic_model(position, orientation, v, w, dt_); + state = optimizer_->kinematic_model(position, orientation, v, w, dt_); p.x = state[0]; p.y = state[1]; p.z = position[2] + 0.5; @@ -159,15 +93,14 @@ MPCController::publish_mpc_path( void MPCController::update_rt(NavState & nav_state) { - if (!nav_state.has("path") || !nav_state.has("robot_pose")) { + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("points")) { if(verbose_) { - std::cout << "No Path or No robot pose" << std::endl; + std::cout << "No Path, No Points or No Robot Pose" << std::endl; } return; } const auto 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; @@ -179,10 +112,25 @@ MPCController::update_rt(NavState & nav_state) } 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 & filtered = PointPerceptionsOpsView(perceptions) + .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) + .fuse("map") + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) + .collapse({NAN, NAN, 0.1}) + .downsample(0.1) + .as_points(); const auto pose = nav_state.get("robot_pose").pose.pose; double roll_, pitch_, yaw_; - tf2::Quaternion q( pose.orientation.x, pose.orientation.y, @@ -192,30 +140,25 @@ MPCController::update_rt(NavState & nav_state) m.getRPY(roll_, pitch_, yaw_); // MPC Code - - MPCParameters params; - params.x0 = {pose.position.x, pose.position.y, pose.position.z}; - params.theta0 = {roll_, pitch_, yaw_}; - 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; - params.goal = Eigen::Vector2d(static_cast(last_pose.x), - static_cast(last_pose.y)); - params.N = horizon_steps_; - params.dt = dt_; - params.R = R_; - params.Q = Q_; - params.Rd = Rd_; - params.qtheta = qtheta_; 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_}, + Q_, + R_, + Rd_, + qtheta_, + static_cast(horizon_steps_), + dt_, + filtered); + + NLoptCallbackData cbdata{ optimizer_.get(), ¶ms }; + nlopt::opt opt(nlopt::LN_COBYLA, static_cast(u.size())); - opt.set_min_objective(nlopt_cost_callback, ¶ms); + opt.set_min_objective(easynav::MPCOptimizer::nlopt_cost_callback, &cbdata); std::vector lb(2 * horizon_steps_); std::vector ub(2 * horizon_steps_); diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp index f0293d6f..322155fc 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -1,296 +1,94 @@ -#include "easynav_mppi_controller/MPCOptimizer.hpp" -#include "tf2/utils.hpp" - -#include -#include -#include -#include - -#include "easynav_common/types/Perceptions.hpp" -#include "easynav_common/types/PointPerception.hpp" +#include "easynav_mpc_controller/MPCOptimizer.hpp" namespace easynav { -MPPIOptimizer::MPPIOptimizer( - double num_samples, double horizon_steps, double dt, double lambda, - double max_lin_vel, double max_ang_vel, double max_lin_acc, double max_ang_acc, - double fov, double safety_radius) -: num_samples_(num_samples), horizon_steps_(horizon_steps), dt_(dt), lambda_(lambda), - max_lin_vel_(max_lin_vel), max_ang_vel_(max_ang_vel), max_lin_acc_(max_lin_acc), - max_ang_acc_(max_ang_acc), - fov_(fov), safety_radius_(safety_radius) -{ -} - -std::vector> -MPPIOptimizer::simulate_trajectory( - double x, double y, double yaw, double v, double w, - const nav_msgs::msg::Path & path, int steps) -{ - size_t horizon_steps = static_cast(steps); - std::vector> trajectory; - trajectory.reserve(horizon_steps); - - bool has_path = !path.poses.empty(); - - for (size_t i = 0; i < horizon_steps; ++i) { - // MPPI noise sampling - double v_sample = v + normal_(rng_); - double w_sample = w + normal_(rng_); - - // Differential drive kinematics - x += v_sample * std::cos(yaw) * dt_; - y += v_sample * std::sin(yaw) * dt_; - yaw += w_sample * dt_; - - if (has_path) { - // Calculate the index in the path based on the current step - size_t path_idx = std::min(static_cast((i * path.poses.size()) / horizon_steps), - path.poses.size() - 1); - double target_x = path.poses[path_idx].pose.position.x; - double target_y = path.poses[path_idx].pose.position.y; - - // Smooth motion towards the target - double alpha = 0.2; - x += alpha * (target_x - x); - y += alpha * (target_y - y); - } +MPCParameters::MPCParameters(Eigen::Vector2d goal, + Eigen::Vector3d x0, + Eigen::Vector3d theta0, + Eigen::Matrix2d Q, + Eigen::Matrix2d R, + Eigen::Matrix2d Rd, + double qtheta, + int N, + double dt, + const pcl::PointCloud & points) + :x0(x0), theta0(theta0), Q(Q), R(R), Rd(Rd), qtheta(qtheta), N(N), + dt(dt), points(points) {} - trajectory.emplace_back(x, y); - } +MPCParameters::~MPCParameters() = default; - return trajectory; -} +MPCOptimizer::MPCOptimizer() {} -double MPPIOptimizer::heading_error( - double robot_yaw, - double target_x, double target_y, - double robot_x, double robot_y) -{ - double target_yaw = std::atan2(target_y - robot_y, target_x - robot_x); - double err = std::atan2(std::sin(target_yaw - robot_yaw), std::cos(target_yaw - robot_yaw)); - return std::abs(err); -} +MPCOptimizer::~MPCOptimizer() = default; -double MPPIOptimizer::shortest_angular_distance(double from, double to) +Eigen::Vector3d +MPCOptimizer::kinematic_model(const Eigen::Vector3d & x, const Eigen::Vector3d & q, double v, double w, double dt) { - double result = std::fmod(to - from + M_PI, 2.0 * M_PI); - if (result < 0) {result += 2.0 * M_PI;} - return result - M_PI; + 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 MPPIOptimizer::compute_cost( - const std::vector> & trajectory, - const nav_msgs::msg::Path & path, - double v, double w, double initial_yaw, - const pcl::PointCloud & points) +double +MPCOptimizer::cost_function(const std::vector & u, std::vector & grad, void *data) { - // Total cost accumulator + MPCParameters *params = reinterpret_cast(data); + + Eigen::Vector3d position = params->x0; + Eigen::Vector3d orientation = params->theta0; + Eigen::Vector3d state; + int N = params->N; + double dt = params->dt; + double qtheta = params->qtheta; double cost = 0.0; - size_t path_size = path.poses.size(); - - // --- Path-Tracking Penalties --- - for (size_t i = 0; i < trajectory.size(); ++i) { - const auto &[x, y] = trajectory[i]; - - // Cost is distributed along the trajectory - double path_alpha = static_cast(i) / trajectory.size(); - size_t idx = std::min(static_cast(path_alpha * path_size), path_size - 1); - - double dx = path.poses[idx].pose.position.x - x; - double dy = path.poses[idx].pose.position.y - y; - double dist = std::hypot(dx, dy); - - // Heading error is calculated based on the initial yaw - double heading_penalty = heading_error(initial_yaw, path.poses[idx].pose.position.x, - path.poses[idx].pose.position.y, x, y); - - // FOV penalty: discourage trajectories outside robot's view - double angle_to_goal = heading_error(initial_yaw, trajectory.back().first, - trajectory.back().second, x, y); - double fov_penalty = std::pow(std::max(0.0, angle_to_goal - fov_), 2); - - // Accumulate penalties - cost += dist; // distance to path - cost += 0.05 * heading_penalty; // heading misalignment - cost += 0.2 * fov_penalty; // leaving field of view - } - - // --- Goal Progress Penalties --- - const auto & goal_pose = path.poses.back().pose; - const double gx = goal_pose.position.x; - const double gy = goal_pose.position.y; - const auto & start_xy = trajectory.front(); - const auto & end_xy = trajectory.back(); - - double d_start_goal = std::hypot(gx - start_xy.first, gy - start_xy.second); - double d_end_goal = std::hypot(gx - end_xy.first, gy - end_xy.second); - - // how much closer we got to goal - double progress = d_start_goal - d_end_goal; - - cost += -2.0 * progress; // reward moving closer to goal - cost += 1.5 * d_end_goal; // penalize being far from goal at end - - // --- Obstacle Avoidance Penalties --- - double min_obs_overall = std::numeric_limits::max(); - - for (const auto & point : points) { - double min_obs_dist = std::numeric_limits::max(); - for (const auto &[x, y] : trajectory) { - double dx = point.x - x; - double dy = point.y - y; - double dist = std::hypot(dx, dy); - if (dist < min_obs_dist) {min_obs_dist = dist;} - } - min_obs_overall = std::min(min_obs_overall, min_obs_dist); - - // Safety margin (robot radius + margin) - if (min_obs_dist < safety_radius_) { - // Heavy penalty for collision risk - cost += 5000.0 * std::pow(safety_radius_ - min_obs_dist, 2) * (1.0 + v); + Eigen::Matrix2d R = params->R; + Eigen::Matrix2d Q = params->Q; + Eigen::Matrix2d Rd = params->Rd; + + 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 { - // Small penalty: encourage keeping clearance - cost += 1.0 / (min_obs_dist * min_obs_dist); + dv = 0.0; + dw = 0.0; } - } - // --- Velocity Encouragement --- - // If obstacles are far, discourage staying too slow - if (min_obs_overall > 0.6) { - cost += 0.2 / std::max(0.05, v); + 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; } - // Encourage smooth motions - double delta_v = v - last_v_; - double delta_w = w - last_w_; - cost += 0.1 * (delta_v * delta_v) + 0.1 * (delta_w * delta_w); - - // --- Regularization --- - // Smooth motions: penalize high linear/angular velocities - cost += 0.002 * (v * v) + 0.005 * (w * w); - return cost; + } -MPPIResult MPPIOptimizer::compute_control( - const geometry_msgs::msg::Pose & current_pose, - const nav_msgs::msg::Path & path, - const pcl::PointCloud & points) +double MPCOptimizer::nlopt_cost_callback( + const std::vector & x, + std::vector & grad, + void *data) { - // If the path is empty, stop the robot - if (path.poses.empty()) { - return MPPIResult{0.0, 0.0, {}, {}}; - } - - // Current robot state - double x = current_pose.position.x; - double y = current_pose.position.y; - double yaw = tf2::getYaw(current_pose.orientation); - - // Select goal pose (within horizon, or last path point) - int last_pose = std::min(static_cast(horizon_steps_), path.poses.size() - 1); - int sim_steps = std::max(1, last_pose); - const auto & path_pose = path.poses[last_pose].pose; - double px = path_pose.position.x; - double py = path_pose.position.y; - double dist_to_goal = std::hypot(px - x, py - y); - - // Compute heading error to the goal - double angle_to_goal = std::atan2(py - y, px - x); - double angle_error = shortest_angular_distance(yaw, angle_to_goal); - - // Initialize sampling - std::vector samples; - samples.reserve(static_cast(num_samples_)); - - std::vector>> all_trajs; - std::vector> best_traj; - - // Base velocity proportional to heading - double angle_mag = std::abs(angle_error); - - // Scale linear velocity based on angular error - double v_scale = 1.0; - if (angle_mag > M_PI / 4.0) { // more than 45° - v_scale = 0.2; // move slowly - } else if (angle_mag > M_PI / 8.0) { // more than 22.5° and less than 45° - v_scale = 0.5; // move moderately - } - - // Base velocities - // Scale linear velocity based on distance to goal: closer to goal, faster we go - double base_v = max_lin_vel_ * std::min(dist_to_goal, 1.0) * v_scale; - base_v = std::clamp(base_v, 0.0, max_lin_vel_); - - // Angular velocity is proportional to the heading error: turn faster if more misaligned - double w_scale = std::min(1.0, 2.0 * angle_mag / M_PI); - double base_w = std::clamp(w_scale * angle_error, -max_ang_vel_, max_ang_vel_); - - // Generate candidate trajectories - for (int i = 0; i < num_samples_; ++i) { - // Sample noise for velocities with Gaussian distribution and clamp - double v = std::clamp(base_v + v_noise_(rng_), -max_lin_vel_, max_lin_vel_); - double w = std::clamp(base_w + w_noise_(rng_), -max_ang_vel_, max_ang_vel_); - - // Simulate trajectory and compute its cost - auto traj = simulate_trajectory(x, y, yaw, v, w, path, sim_steps); - double cost = compute_cost(traj, path, v, w, yaw, points); - all_trajs.push_back(traj); - - // Store the sample with its cost - samples.push_back({v, w, cost}); - } - - // Softmin: Find minimum cost among samples - auto best_sample_it = std::min_element(samples.begin(), samples.end(), - [](const auto & a, const auto & b) {return a.cost < b.cost;}); - - // Best trajectory and cost - best_traj = all_trajs[std::distance(samples.begin(), best_sample_it)]; - double min_cost = best_sample_it->cost; - - // Adapt lambda (temperature) if velocities collapse to near zero - double min_v_sample = std::numeric_limits::max(); - for (const auto & s : samples) { - min_v_sample = std::min(min_v_sample, s.v); - } - - if (min_v_sample < 0.05) { - lambda_ = std::min(5.0, lambda_ * 1.2); // increase lambda if tends to zero (stop) - } - - // Softmin weighting of samples - double denom = 0.0; - for (auto & sample : samples) { - sample.cost = std::exp(-1.0 / lambda_ * (sample.cost - min_cost)); - denom += sample.cost; - } - - // Weighted average of velocities - double vlin = 0.0, vrot = 0.0; - for (const auto & sample : samples) { - vlin += sample.v * sample.cost / denom; - vrot += sample.w * sample.cost / denom; - } - - // Calculate the maximum change in velocities based on acceleration limits - double max_v_delta = max_lin_acc_ * dt_; - double max_w_delta = max_ang_acc_ * dt_; - - // Limit velocity changes (slew rate) - vlin = last_v_ + std::clamp(vlin - last_v_, -max_v_delta, max_v_delta); - vrot = last_w_ + std::clamp(vrot - last_w_, -max_w_delta, max_w_delta); - - // Smooth velocities - double alpha_smooth = 0.2; // lower is more smooth - last_v_ = alpha_smooth * vlin + (1.0 - alpha_smooth) * last_v_; - last_w_ = alpha_smooth * vrot + (1.0 - alpha_smooth) * last_w_; - - // Return final control command and trajectories - return MPPIResult{last_v_, last_w_, all_trajs, best_traj}; + auto *cbdata = static_cast(data); + return cbdata->optimizer->cost_function(x, grad, cbdata->params); } } // namespace easynav From f15fcec207b15f91d047add7fb5b7b444c225168 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 26 Nov 2025 21:12:49 +0100 Subject: [PATCH 076/136] Methods were added to access to private attributes --- .../easynav_mpc_controller/MPCController.hpp | 4 -- .../easynav_mpc_controller/MPCOptimizer.hpp | 30 ++++++----- .../easynav_mpc_controller/MPCController.cpp | 8 +-- .../easynav_mpc_controller/MPCOptimizer.cpp | 53 ++++++++++++++----- 4 files changed, 59 insertions(+), 36 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index d34f3de1..41046999 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -68,10 +68,6 @@ class MPCController : public ControllerMethodBase rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path markers. private: - Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost - Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost - Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost - double qtheta_ {3.0}; geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. }; diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp index 0197e019..6893b0ba 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -23,26 +23,32 @@ class MPCParameters MPCParameters(Eigen::Vector2d goal, Eigen::Vector3d x0, Eigen::Vector3d theta0, - Eigen::Matrix2d Q, - Eigen::Matrix2d R, - Eigen::Matrix2d Rd, - double qtheta, + const pcl::PointCloud & points, int N, - double dt, - const pcl::PointCloud & points); + double dt); ~MPCParameters(); Eigen::Vector2d goal; Eigen::Vector3d x0; Eigen::Vector3d theta0; - Eigen::Matrix2d Q; - Eigen::Matrix2d R; - Eigen::Matrix2d Rd; - double qtheta; - int N; - double dt; const pcl::PointCloud & points; + + int get_steps(); + double get_timestep(); + double get_angular_tracking_cost(); + Eigen::Matrix2d get_effort_cost(); + Eigen::Matrix2d get_tracking_cost(); + Eigen::Matrix2d get_smooth_cost(); + +private: + int N_ {5}; + double dt_ {0.1}; + Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost + Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost + Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost + double qtheta_ {3.0}; + }; class MPCOptimizer diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index c35d45b7..fcfd0695 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -147,13 +147,9 @@ MPCController::update_rt(NavState & nav_state) Eigen::Vector2d(static_cast(last_pose.x), static_cast(last_pose.y)), {pose.position.x, pose.position.y, pose.position.z}, {roll_, pitch_, yaw_}, - Q_, - R_, - Rd_, - qtheta_, + filtered, static_cast(horizon_steps_), - dt_, - filtered); + dt_); NLoptCallbackData cbdata{ optimizer_.get(), ¶ms }; diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp index 322155fc..a5b373bd 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -6,18 +6,43 @@ namespace easynav MPCParameters::MPCParameters(Eigen::Vector2d goal, Eigen::Vector3d x0, Eigen::Vector3d theta0, - Eigen::Matrix2d Q, - Eigen::Matrix2d R, - Eigen::Matrix2d Rd, - double qtheta, + const pcl::PointCloud & points, int N, - double dt, - const pcl::PointCloud & points) - :x0(x0), theta0(theta0), Q(Q), R(R), Rd(Rd), qtheta(qtheta), N(N), - dt(dt), points(points) {} + 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; @@ -40,14 +65,14 @@ MPCOptimizer::cost_function(const std::vector & u, std::vector & Eigen::Vector3d position = params->x0; Eigen::Vector3d orientation = params->theta0; Eigen::Vector3d state; - int N = params->N; - double dt = params->dt; - double qtheta = params->qtheta; + 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->R; - Eigen::Matrix2d Q = params->Q; - Eigen::Matrix2d Rd = params->Rd; + 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]; From 3ba63bc1cc131c7b5287237d71caa0a223481340 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 27 Nov 2025 01:27:02 +0100 Subject: [PATCH 077/136] Obstacles are detected --- .../easynav_mpc_controller/MPCOptimizer.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp index a5b373bd..4e8df5bb 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -69,6 +69,7 @@ MPCOptimizer::cost_function(const std::vector & u, std::vector & double dt = params->get_timestep(); double qtheta = params->get_angular_tracking_cost(); double cost = 0.0; + double penalty = 0.25; Eigen::Matrix2d R = params->get_effort_cost(); Eigen::Matrix2d Q = params->get_tracking_cost(); @@ -94,6 +95,30 @@ MPCOptimizer::cost_function(const std::vector & u, std::vector & Eigen::Vector2d uk(v, w); Eigen::Vector2d duk(dv, dw); + for (const auto & point : params->points) { + // double min_obs_dist = std::numeric_limits::max(); + // for (const auto &[x, y] : trajectory) { + // double dx = point.x - x; + // double dy = point.y - y; + // double dist = std::hypot(dx, dy); + // if (dist < min_obs_dist) {min_obs_dist = dist;} + // } + // min_obs_overall = std::min(min_obs_overall, min_obs_dist); + + // // Safety margin (robot radius + margin) + // if (min_obs_dist < safety_radius_) { + // // Heavy penalty for collision risk + // cost += 5000.0 * std::pow(safety_radius_ - min_obs_dist, 2) * (1.0 + v); + // } else { + // // Small penalty: encourage keeping clearance + // cost += 1.0 / (min_obs_dist * min_obs_dist); + double dist = std::hypot(point.x - pos[0] , point.y -pos[1]); + if (dist < 1.0){ + cost+=penalty * std::pow(1.0 - dist, 2) * std::pow(v, 2); + //cost+=penalty; + } + } + // Tracking cost cost += Q(0, 0) * error[0] * error[0] + Q(1, 1) * error[1] * error[1] + qtheta * error_theta * error_theta; From 3d93aae3acf6f5432fbad76a5f0c66ed02c97202 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 28 Nov 2025 07:19:07 +0100 Subject: [PATCH 078/136] Referencing base class if ot void MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/README.md | 3 +++ controllers/easynav_serest_controller/README.md | 3 +++ controllers/easynav_simple_controller/README.md | 3 +++ controllers/easynav_vff_controller/README.md | 3 +++ 4 files changed, 12 insertions(+) diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 4daac078..6ed9bd95 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -28,6 +28,9 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig ## 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. | diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index 0b2e200c..0741d499 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -28,6 +28,9 @@ A SeReST (Smooth Error-Responsive Speed and Turning) controller for path trackin ## 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²). | diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index bf139bfc..37c54c78 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -28,6 +28,9 @@ Simple path-following controller that uses PID controllers and a look-ahead refe ## 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). diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index b2e54ea7..b0fb3631 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -28,6 +28,9 @@ Vector Field Histogram (VFF) style local obstacle avoidance controller. Generate ## 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 From ddc7295dae17bc9626fae145af1a08a5190f5725 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 28 Nov 2025 07:34:55 +0100 Subject: [PATCH 079/136] linting docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/README.md | 9 ++++ .../easynav_serest_controller/README.md | 13 +++++- .../easynav_simple_controller/README.md | 46 +++++++++++-------- controllers/easynav_vff_controller/README.md | 12 ++++- 4 files changed, 57 insertions(+), 23 deletions(-) diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 6ed9bd95..b8eb626d 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -7,10 +7,12 @@ 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) | @@ -19,6 +21,7 @@ A Model Predictive Path Integral (MPPI) controller implementation for Easy Navig | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_mppi_controller/MPPIController` - **Type:** `easynav::MPPIController` - **Base Class:** `easynav::ControllerMethodBase` @@ -26,6 +29,7 @@ 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). \ @@ -48,6 +52,7 @@ All parameters are declared under the plugin namespace, i.e., `//easyn ## 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 | @@ -55,10 +60,12 @@ All parameters are declared under the plugin namespace, i.e., `//easyn ### 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. | @@ -68,7 +75,9 @@ This package does not create service servers or clients. ## TF Frames + This controller does not explicitly publish or require TF frames in code. ## License + GPL-3.0-only diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index 0741d499..2a405c28 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -4,21 +4,25 @@ ## 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) | +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_serest_controller/SerestController` - **Type:** `easynav::SerestController` - **Base Class:** `easynav::ControllerMethodBase` @@ -26,6 +30,7 @@ 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). \ @@ -76,13 +81,15 @@ All parameters are declared under the plugin namespace, i.e., `//easyn ## 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. | @@ -106,7 +113,9 @@ This package does not create service servers or clients. ## 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_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index 37c54c78..75116d90 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -4,13 +4,16 @@ ## Description + 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) | @@ -19,6 +22,7 @@ Simple path-following controller that uses PID controllers and a look-ahead refe | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_simple_controller/SimpleController` - **Type:** `easynav::SimpleController` - **Base Class:** `easynav::ControllerMethodBase` @@ -26,6 +30,7 @@ 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). \ @@ -33,38 +38,41 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | 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_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index b0fb3631..68109a00 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -4,13 +4,16 @@ ## Description + 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) | @@ -19,6 +22,7 @@ Vector Field Histogram (VFF) style local obstacle avoidance controller. Generate | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_vff_controller/VffController` **Type:** `easynav::VffController` **Base Class:** `easynav::ControllerMethodBase` @@ -26,6 +30,7 @@ Vector Field Histogram (VFF) style local obstacle avoidance controller. Generate **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). \ @@ -34,6 +39,7 @@ No ROS parameters are currently declared in code. (Add declarations in the plugi ## Interfaces ### NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current pose for steering decisions. | @@ -41,17 +47,19 @@ No ROS parameters are currently declared in code. (Add declarations in the plugi | `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. - +Relies on frames stamped in `robot_pose`; does not query TF directly. ## License + GPL-3.0-only From ad0261db454c0d5b2852c4134f5f3e98816be23f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 28 Nov 2025 07:38:19 +0100 Subject: [PATCH 080/136] linting docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/README.md | 6 +----- controllers/easynav_serest_controller/README.md | 3 --- controllers/easynav_simple_controller/README.md | 1 - controllers/easynav_vff_controller/README.md | 1 - 4 files changed, 1 insertion(+), 10 deletions(-) diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index b8eb626d..80028786 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -2,8 +2,8 @@ [![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 Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. ## Authors and Maintainers @@ -48,7 +48,6 @@ 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 @@ -58,12 +57,10 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | 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. - ## NavState Keys | Key | Type | Access | Notes | @@ -73,7 +70,6 @@ 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. diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index 2a405c28..017a29ea 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -2,7 +2,6 @@ [![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 SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. @@ -77,7 +76,6 @@ 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 @@ -111,7 +109,6 @@ 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. diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index 75116d90..08b8a5e3 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -2,7 +2,6 @@ [![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 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. diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index 68109a00..3a911989 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -2,7 +2,6 @@ [![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 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. From 9472199e4f24d5593ae8ead0222e1159a2318f47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 28 Nov 2025 07:47:24 +0100 Subject: [PATCH 081/136] linting docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/README.md | 2 -- .../easynav_serest_controller/README.md | 2 -- .../easynav_simple_controller/README.md | 2 -- controllers/easynav_vff_controller/README.md | 2 -- localizers/easynav_costmap_localizer/README.md | 16 ++++++++++------ localizers/easynav_gps_localizer/README.md | 15 ++++++++++----- localizers/easynav_navmap_localizer/README.md | 12 ++++++++++-- localizers/easynav_simple_localizer/README.md | 16 ++++++++++------ .../easynav_bonxai_maps_manager/README.md | 17 ++++++++++------- .../easynav_costmap_maps_manager/README.md | 17 ++++++++++++++--- .../easynav_navmap_maps_manager/README.md | 14 ++++++++++++-- .../easynav_octomap_maps_manager/README.md | 17 ++++++++++++++--- .../easynav_simple_maps_manager/README.md | 18 +++++++++++------- 13 files changed, 101 insertions(+), 49 deletions(-) diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 80028786..cf0f9e9f 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -1,7 +1,5 @@ # easynav_mppi_controller -[![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 Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index 017a29ea..b4b64b68 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -1,7 +1,5 @@ # easynav_serest_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)](#) [![ROS 2: jazzy](https://img.shields.io/badge/ROS%202-jazzy-blue)](#) - ## Description A SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index 08b8a5e3..3f8f03ee 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -1,7 +1,5 @@ # easynav_simple_controller -[![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 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. diff --git a/controllers/easynav_vff_controller/README.md b/controllers/easynav_vff_controller/README.md index 3a911989..299a1958 100644 --- a/controllers/easynav_vff_controller/README.md +++ b/controllers/easynav_vff_controller/README.md @@ -1,7 +1,5 @@ # easynav_vff_controller -[![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 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. diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index 9f108626..8e5f5e7b 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -1,15 +1,16 @@ # easynav_costmap_localizer -[![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 + 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) | @@ -18,6 +19,7 @@ AMCL-style localizer using a 2D Costmap2D for scoring and odometry/IMU for predi | 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` @@ -25,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 | @@ -43,10 +46,10 @@ 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) | @@ -54,23 +57,24 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | 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_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index b3b44698..4c45be17 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -1,15 +1,16 @@ # easynav_gps_localizer -[![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 + 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) | @@ -18,6 +19,7 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- | 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` @@ -25,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_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index c2229864..80f2effe 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -1,15 +1,16 @@ # easynav_navmap_localizer -[![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 + 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) | @@ -18,6 +19,7 @@ Easy Navigation: nAVmAP Localizer package. | 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` @@ -25,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 | @@ -52,6 +55,7 @@ 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) | @@ -59,9 +63,11 @@ All parameters are declared under the plugin name namespace, i.e., `// | 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. | @@ -72,6 +78,7 @@ 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. | @@ -79,4 +86,5 @@ This package does not create service servers or clients. | 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_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 2de8ce67..481d6830 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -1,15 +1,16 @@ # easynav_simple_localizer -[![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 + 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) | @@ -18,6 +19,7 @@ AMCL-style localizer using a simple 2D grid-like map for scoring and odometry fo | 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` @@ -25,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 | @@ -42,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/maps_managers/easynav_bonxai_maps_manager/README.md b/maps_managers/easynav_bonxai_maps_manager/README.md index bb3c03c9..6f08a413 100644 --- a/maps_managers/easynav_bonxai_maps_manager/README.md +++ b/maps_managers/easynav_bonxai_maps_manager/README.md @@ -1,15 +1,16 @@ # easynav_bonxai_maps_manager -[![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 + 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) | @@ -18,6 +19,7 @@ Maps Manager that maintains a [Bonxai](https://github.com/facontidavide/Bonxai) | 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` @@ -27,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. | @@ -35,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_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 3d52c7c4..9f2d3091 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -1,11 +1,10 @@ # easynav_costmap_maps_manager -[![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 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,10 +12,12 @@ 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) | @@ -25,6 +26,7 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends | 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` @@ -36,6 +38,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`. | @@ -46,9 +49,11 @@ At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends --- ### Filter Plugins + 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:** @@ -70,6 +75,7 @@ Each entry in `.filters` defines a sub-namespace `.` wit | `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:** @@ -95,7 +101,7 @@ Each entry in `.filters` defines a sub-namespace `.` wit **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: @@ -119,6 +125,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` | @@ -126,6 +133,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. | @@ -133,6 +141,7 @@ maps_manager_node: --- ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `map.static` | `Costmap2D` | **Write** | Static map loaded from YAML. | @@ -143,6 +152,7 @@ maps_manager_node: --- ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | — | This manager does not broadcast TF; costmaps use their internal `frame_id`. | @@ -150,4 +160,5 @@ maps_manager_node: --- ## License + GPL-3.0-only diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index 2a45f1cb..e395db8d 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -1,17 +1,17 @@ # easynav_navmap_maps_manager -[![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 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 - **Maintainer:** Francisco Martín Rico ## Supported ROS 2 Distributions + | Distribution | Status | |---|---| | humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | @@ -20,6 +20,7 @@ This package also includes map filters implemented as plugins (`ObstacleFilter` | rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | ## Plugin (pluginlib) + - **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager` - **Type:** `easynav::navmap::NavMapMapsManager` - **Base Class:** `easynav::MapsManagerBase` @@ -28,6 +29,7 @@ This package also includes map filters implemented as plugins (`ObstacleFilter` 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/...` @@ -43,6 +45,7 @@ All parameters are declared under the plugin namespace, i.e. ### Filter Plugins #### **ObstacleFilter** + - **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/ObstacleFilter` - **Type:** `easynav::navmap::ObstacleFilter` - **Description:** @@ -59,6 +62,7 @@ All parameters are declared under the plugin namespace, i.e. | **Output Layer:** | | | Updates or creates NavMap layer `"obstacles"`. | #### **InflationFilter** + - **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/InflationFilter` - **Type:** `easynav::navmap::InflationFilter` - **Description:** @@ -76,6 +80,7 @@ All parameters are declared under the plugin namespace, i.e. ## Interfaces (Topics and Services) ### Subscriptions and Publications + | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Publisher | `//map` | `navmap_ros_interfaces/msg/NavMap` | Publishes the full NavMap. | depth=1 | @@ -84,22 +89,26 @@ All parameters are declared under the plugin namespace, i.e. | 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` | Saves the current NavMap and layers to disk. | ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `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. | ## Example Configuration + ```yaml maps_manager_node: ros__parameters: @@ -121,4 +130,5 @@ maps_manager_node: ``` ## License + GPL-3.0-only diff --git a/maps_managers/easynav_octomap_maps_manager/README.md b/maps_managers/easynav_octomap_maps_manager/README.md index 9ea021d4..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` @@ -27,12 +29,15 @@ Maps Manager that maintains an [OctoMap](https://octomap.github.io/) (probabilis ## Parameters ### 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. | @@ -40,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: @@ -67,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 | ### Services + | Direction | Service | Type | Purpose | |---|---|---|---| | Service Server | `//savemap` | `std_srvs/srv/Trigger` | Saves the current OctoMap to disk. | @@ -80,6 +88,7 @@ maps_manager_node: --- ## NavState Keys + | Key | Type | Access | Notes | |---|---|---|---| | `map` | `::octomap::Octomap` | **Read** | If present in NavState, used as an input/seed map. (Plugin currently does not write back to NavState.) | @@ -87,6 +96,7 @@ maps_manager_node: --- ## TF Frames + | Role | Transform | Notes | |---|---|---| | Publishes | — | This manager does not broadcast TF; outputs are stamped using the configured frame(s) in the map data. | @@ -94,4 +104,5 @@ maps_manager_node: --- ## License + GPL-3.0-only diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 4dad4744..4590daf1 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -1,17 +1,18 @@ # easynav_simple_maps_manager -[![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 + 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) | @@ -20,6 +21,7 @@ At the heart of this stack is the SimpleMap data structure. It represents the en | 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` @@ -27,40 +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/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 From 740d9fcde9c0ba73ccadabd6e1294989b2c8e4ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 28 Nov 2025 07:47:45 +0100 Subject: [PATCH 082/136] linting docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- maps_managers/easynav_costmap_maps_manager/README.md | 1 + maps_managers/easynav_navmap_maps_manager/README.md | 1 + 2 files changed, 2 insertions(+) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 9f2d3091..d2df322a 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -1,6 +1,7 @@ # easynav_costmap_maps_manager ## 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]: diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index e395db8d..2f223f28 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -1,6 +1,7 @@ # easynav_navmap_maps_manager ## 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. From 3c62c283cee8d98bb558d3f7af3f8863dbbc850e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 28 Nov 2025 07:48:39 +0100 Subject: [PATCH 083/136] linting docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- localizers/easynav_navmap_localizer/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index 80f2effe..e1451c27 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -58,7 +58,7 @@ All parameters are declared under the plugin name namespace, i.e., `// | 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 | @@ -82,7 +82,7 @@ This package does not create service servers or clients. | 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 From 1cb8df1a526ce0872bbfe046f3074f62b7c97067 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 28 Nov 2025 07:49:16 +0100 Subject: [PATCH 084/136] linting docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index ea126c17..93d70863 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,7 @@ 📋 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. @@ -19,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 | @@ -30,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 | @@ -42,6 +45,7 @@ Motion controllers for trajectory tracking and reactive behaviors. --- ### 🗺️ Maps Managers + Map management plugins that provide, update, and store different environment representations. | Package | Description | Link | @@ -55,6 +59,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 | @@ -67,5 +72,5 @@ Localization plugins based on different map types and sensors. --- ## 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. From 565e5cf1c467565631d91a8db432d2d041198b97 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 28 Nov 2025 17:52:57 +0100 Subject: [PATCH 085/136] Works. Documentation checked --- controllers/easynav_mpc_controller/README.md | 14 ++- .../easynav_mpc_controller/MPCController.hpp | 37 +++++--- .../easynav_mpc_controller/MPCOptimizer.hpp | 66 +++++++++++--- .../easynav_mpc_controller/MPCController.cpp | 91 ++++++++++++------- .../easynav_mpc_controller/MPCOptimizer.cpp | 70 +++++++------- 5 files changed, 180 insertions(+), 98 deletions(-) diff --git a/controllers/easynav_mpc_controller/README.md b/controllers/easynav_mpc_controller/README.md index 59b2a9d9..a707baee 100644 --- a/controllers/easynav_mpc_controller/README.md +++ b/controllers/easynav_mpc_controller/README.md @@ -27,16 +27,21 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | Name | Type | Default | Description | |---|---|---:|---| -| `.horizon_steps` | `int` | `10` | Number of time steps in the prediction horizon. | +| `.horizon_steps` | `int` | `5` | Number of time steps in the prediction horizon. | | `.dt` | `double` | `0.1` | Integration time step (seconds). | -| `.max_linear_velocity` | `double` | `1.0` | Maximum linear velocity (m/s). | -| `.max_angular_velocity` | `double` | `1.0` | Maximum angular velocity (rad/s). | +| `.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 -This package does not create topic subscriptions or 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 @@ -49,6 +54,7 @@ This package does not create service servers or clients. | `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 diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 41046999..7f15a609 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -20,8 +20,11 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" -#include "visualization_msgs/msg/marker.hpp" -#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" @@ -50,22 +53,28 @@ class MPCController : public ControllerMethodBase void update_rt(NavState & nav_state) override; /// \brief Publishes the selected path by MPC controller - /// \param pose Eigen::Vector3d of the robot - /// \param best_path Path generated by MPC - void publish_mpc_path( - const Eigen::Vector3d & position, const Eigen::Vector3d & orientation, - const std::vector & best_path); + /// \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 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 + 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 - std::unique_ptr optimizer_; ///< MPPI optimizer + std::unique_ptr optimizer_; ///< MPC optimizer - rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path markers. + 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. diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp index 6893b0ba..c56d7e08 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -16,10 +16,10 @@ namespace easynav { +/// \brief A MPC parameters class. class MPCParameters { public: - MPCParameters(Eigen::Vector2d goal, Eigen::Vector3d x0, Eigen::Vector3d theta0, @@ -27,53 +27,89 @@ class MPCParameters int N, double dt); + /// \brief Destructor. ~MPCParameters(); - Eigen::Vector2d goal; - Eigen::Vector3d x0; - Eigen::Vector3d theta0; - const pcl::PointCloud & points; + 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}; - double dt_ {0.1}; - Eigen::Matrix2d Q_ {{4.0, 0.0}, {0.0, 4.0}}; ///< Tracking Cost - Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost - Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost - double qtheta_ {3.0}; + 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); - double cost_function(const std::vector & u, + /// \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; - MPCParameters *params; + MPCOptimizer *optimizer; ///< Pointer to optimizer. + MPCParameters *params; ///< Pointer to parameter for optimizer. }; } // namespace easynav diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index fcfd0695..1347a97e 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -37,12 +37,14 @@ MPCController::on_initialize() 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->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_); @@ -50,44 +52,61 @@ MPCController::on_initialize() optimizer_ = std::make_unique(); mpc_path_pub_ = - node->create_publisher("/mpc/path", 10); + node->create_publisher("/mpc/path", 10); + + detection_pub_ = + node->create_publisher("/mpc/detection",10); return {}; } void -MPCController::publish_mpc_path( - const Eigen::Vector3d & position, - const Eigen::Vector3d & orientation, const std::vector & best_vel) +MPCController::publish_mpc_path(void *data, const std::vector & best_vel, nav_msgs::msg::Path path) { - visualization_msgs::msg::MarkerArray path; - visualization_msgs::msg::Marker points; - points.header.frame_id = "map"; - points.header.stamp = rclcpp::Clock().now(); - points.ns = "mpc_path"; - points.id = 0; - points.type = visualization_msgs::msg::Marker::LINE_STRIP; - points.action = visualization_msgs::msg::Marker::ADD; - points.scale.x = 0.05; - points.color.r = 1.0; - points.color.g = 0.0; - points.color.b = 0.0; - points.color.a = 0.8; - - Eigen::Vector3d state; - for (size_t i = 0; i + 1 < best_vel.size(); i += 2) { - double v = best_vel[i]; - double w = best_vel[i + 1]; - geometry_msgs::msg::Point p; - state = optimizer_->kinematic_model(position, orientation, v, w, dt_); - p.x = state[0]; - p.y = state[1]; - p.z = position[2] + 0.5; - points.points.push_back(p); + 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_); + } +} - path.markers.push_back(points); - mpc_path_pub_->publish(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++; + } + } + 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(real_points!=0 && dist < safety_radius_){ + std::cerr << "Detection at: " << dist << " Theta: " << angle << std::endl; + u[0] -= dist/real_points * dt_; + u[1] -= angle/real_points * dt_; + } } void @@ -122,13 +141,19 @@ MPCController::update_rt(NavState & nav_state) const auto & perceptions = nav_state.get("points"); const auto & filtered = PointPerceptionsOpsView(perceptions) - .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0}) + .filter({-2.0, -0.35, -1.0}, {0.0, 0.35, 1.0}) .fuse("map") .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( @@ -185,6 +210,8 @@ MPCController::update_rt(NavState & nav_state) std::cerr << "Optimization Error: " << e.what() << std::endl; } + collision_checker(¶ms, u); + // Publish the computed velocity command cmd_vel_.header.frame_id = path.header.frame_id; cmd_vel_.header.stamp = get_node()->now(); @@ -194,7 +221,7 @@ MPCController::update_rt(NavState & nav_state) nav_state.set("cmd_vel", cmd_vel_); // Publish the path - publish_mpc_path(params.x0, params.theta0, u); + publish_mpc_path(¶ms, u, path); } } // namespace easynav diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp index 4e8df5bb..bc1c6e36 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -1,3 +1,25 @@ +// 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 @@ -13,32 +35,38 @@ MPCParameters::MPCParameters(Eigen::Vector2d goal, MPCParameters::~MPCParameters() = default; -int MPCParameters::get_steps() +int +MPCParameters::get_steps() { return N_; } -double MPCParameters::get_timestep() +double +MPCParameters::get_timestep() { return dt_; } -double MPCParameters::get_angular_tracking_cost() +double +MPCParameters::get_angular_tracking_cost() { return qtheta_; } -Eigen::Matrix2d MPCParameters::get_effort_cost() +Eigen::Matrix2d +MPCParameters::get_effort_cost() { return R_; } -Eigen::Matrix2d MPCParameters::get_tracking_cost() +Eigen::Matrix2d +MPCParameters::get_tracking_cost() { return Q_; } -Eigen::Matrix2d MPCParameters::get_smooth_cost() +Eigen::Matrix2d +MPCParameters::get_smooth_cost() { return Rd_; } @@ -58,7 +86,7 @@ MPCOptimizer::kinematic_model(const Eigen::Vector3d & x, const Eigen::Vector3d & } double -MPCOptimizer::cost_function(const std::vector & u, std::vector & grad, void *data) +MPCOptimizer::cost_function(const std::vector & u, [[maybe_unused]] std::vector & grad, void *data) { MPCParameters *params = reinterpret_cast(data); @@ -69,7 +97,6 @@ MPCOptimizer::cost_function(const std::vector & u, std::vector & double dt = params->get_timestep(); double qtheta = params->get_angular_tracking_cost(); double cost = 0.0; - double penalty = 0.25; Eigen::Matrix2d R = params->get_effort_cost(); Eigen::Matrix2d Q = params->get_tracking_cost(); @@ -95,30 +122,6 @@ MPCOptimizer::cost_function(const std::vector & u, std::vector & Eigen::Vector2d uk(v, w); Eigen::Vector2d duk(dv, dw); - for (const auto & point : params->points) { - // double min_obs_dist = std::numeric_limits::max(); - // for (const auto &[x, y] : trajectory) { - // double dx = point.x - x; - // double dy = point.y - y; - // double dist = std::hypot(dx, dy); - // if (dist < min_obs_dist) {min_obs_dist = dist;} - // } - // min_obs_overall = std::min(min_obs_overall, min_obs_dist); - - // // Safety margin (robot radius + margin) - // if (min_obs_dist < safety_radius_) { - // // Heavy penalty for collision risk - // cost += 5000.0 * std::pow(safety_radius_ - min_obs_dist, 2) * (1.0 + v); - // } else { - // // Small penalty: encourage keeping clearance - // cost += 1.0 / (min_obs_dist * min_obs_dist); - double dist = std::hypot(point.x - pos[0] , point.y -pos[1]); - if (dist < 1.0){ - cost+=penalty * std::pow(1.0 - dist, 2) * std::pow(v, 2); - //cost+=penalty; - } - } - // Tracking cost cost += Q(0, 0) * error[0] * error[0] + Q(1, 1) * error[1] * error[1] + qtheta * error_theta * error_theta; @@ -132,7 +135,8 @@ MPCOptimizer::cost_function(const std::vector & u, std::vector & } -double MPCOptimizer::nlopt_cost_callback( +double +MPCOptimizer::nlopt_cost_callback( const std::vector & x, std::vector & grad, void *data) From 5ae47758565190e74c285bf053a3f0b87d87bf27 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 28 Nov 2025 18:05:57 +0100 Subject: [PATCH 086/136] Uncrustify corrected --- .../easynav_mpc_controller/MPCController.hpp | 4 ++- .../easynav_mpc_controller/MPCOptimizer.hpp | 30 +++++++++++-------- .../easynav_mpc_controller/MPCController.cpp | 20 +++++++------ .../easynav_mpc_controller/MPCOptimizer.cpp | 29 ++++++++++-------- 4 files changed, 48 insertions(+), 35 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 7f15a609..51ec31d6 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -56,7 +56,9 @@ class MPCController : public ControllerMethodBase /// \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); + 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 diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp index c56d7e08..230ed320 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -20,7 +20,8 @@ namespace easynav class MPCParameters { public: - MPCParameters(Eigen::Vector2d goal, + MPCParameters( + Eigen::Vector2d goal, Eigen::Vector3d x0, Eigen::Vector3d theta0, const pcl::PointCloud & points, @@ -43,7 +44,7 @@ class MPCParameters /// \return double value of time in seconds double get_timestep(); - /// \brief Get angular cost used in angle optimization + /// \brief Get angular cost used in angle optimization /// \return double value of angular cost double get_angular_tracking_cost(); @@ -81,16 +82,17 @@ class 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 v double lineal velocity /// \param w double angular velocity - /// \param dt double differential time used for integration + /// \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); + 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 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]] @@ -98,19 +100,21 @@ class MPCOptimizer /// \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 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, + 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. - }; +struct NLoptCallbackData +{ + MPCOptimizer *optimizer; ///< Pointer to optimizer. + MPCParameters *params; ///< Pointer to parameter for optimizer. +}; } // namespace easynav diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 1347a97e..373b1da4 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -55,13 +55,15 @@ MPCController::on_initialize() node->create_publisher("/mpc/path", 10); detection_pub_ = - node->create_publisher("/mpc/detection",10); + node->create_publisher("/mpc/detection", 10); return {}; } void -MPCController::publish_mpc_path(void *data, const std::vector & best_vel, nav_msgs::msg::Path path) +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_; @@ -92,20 +94,20 @@ MPCController::collision_checker(void *data, std::vector & u) 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)){ + 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++; } } - x_m/=real_points; - y_m/=real_points; + 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(real_points!=0 && dist < safety_radius_){ + if(real_points != 0 && dist < safety_radius_) { std::cerr << "Detection at: " << dist << " Theta: " << angle << std::endl; - u[0] -= dist/real_points * dt_; - u[1] -= angle/real_points * dt_; + u[0] -= dist / real_points * dt_; + u[1] -= angle / real_points * dt_; } } @@ -176,7 +178,7 @@ MPCController::update_rt(NavState & nav_state) static_cast(horizon_steps_), dt_); - NLoptCallbackData cbdata{ optimizer_.get(), ¶ms }; + 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); diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp index bc1c6e36..123f1b3f 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -25,47 +25,48 @@ namespace easynav { -MPCParameters::MPCParameters(Eigen::Vector2d goal, +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) {} +:goal(goal), x0(x0), theta0(theta0), points(points), N_(N), dt_(dt) {} MPCParameters::~MPCParameters() = default; -int +int MPCParameters::get_steps() { return N_; } -double +double MPCParameters::get_timestep() { return dt_; } -double +double MPCParameters::get_angular_tracking_cost() { return qtheta_; } -Eigen::Matrix2d +Eigen::Matrix2d MPCParameters::get_effort_cost() { return R_; } -Eigen::Matrix2d +Eigen::Matrix2d MPCParameters::get_tracking_cost() { return Q_; } -Eigen::Matrix2d +Eigen::Matrix2d MPCParameters::get_smooth_cost() { return Rd_; @@ -76,7 +77,9 @@ MPCOptimizer::MPCOptimizer() {} MPCOptimizer::~MPCOptimizer() = default; Eigen::Vector3d -MPCOptimizer::kinematic_model(const Eigen::Vector3d & x, const Eigen::Vector3d & q, double v, double w, double dt) +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; @@ -86,7 +89,9 @@ MPCOptimizer::kinematic_model(const Eigen::Vector3d & x, const Eigen::Vector3d & } double -MPCOptimizer::cost_function(const std::vector & u, [[maybe_unused]] std::vector & grad, void *data) +MPCOptimizer::cost_function( + const std::vector & u, + [[maybe_unused]] std::vector & grad, void *data) { MPCParameters *params = reinterpret_cast(data); @@ -135,13 +140,13 @@ MPCOptimizer::cost_function(const std::vector & u, [[maybe_unused]] std: } -double +double MPCOptimizer::nlopt_cost_callback( const std::vector & x, std::vector & grad, void *data) { - auto *cbdata = static_cast(data); + auto *cbdata = static_cast(data); return cbdata->optimizer->cost_function(x, grad, cbdata->params); } From a43a3a599fcc1f7cdca25c5d9227ee861c0a295e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 30 Nov 2025 08:46:49 +0100 Subject: [PATCH 087/136] Basic route server MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_routes_maps_manager/CHANGELOG.rst | 8 + .../CMakeLists.txt | 88 +++++++++ .../easynav_routes_maps_manager/README.md | 54 ++++++ .../easynav_routes_maps_manager_plugins.xml | 11 ++ .../RoutesMapsManager.hpp | 110 +++++++++++ .../easynav_routes_maps_manager/package.xml | 33 ++++ .../RoutesMapsManager.cpp | 181 ++++++++++++++++++ .../tests/CMakeLists.txt | 11 ++ .../tests/routes_mapsmanager_tests.cpp | 156 +++++++++++++++ 9 files changed, 652 insertions(+) create mode 100644 maps_managers/easynav_routes_maps_manager/CHANGELOG.rst create mode 100644 maps_managers/easynav_routes_maps_manager/CMakeLists.txt create mode 100644 maps_managers/easynav_routes_maps_manager/README.md create mode 100644 maps_managers/easynav_routes_maps_manager/easynav_routes_maps_manager_plugins.xml create mode 100644 maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp create mode 100644 maps_managers/easynav_routes_maps_manager/package.xml create mode 100644 maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp create mode 100644 maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt create mode 100644 maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp 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..f848cc1e --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt @@ -0,0 +1,88 @@ +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() + +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(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) + + +add_library(${PROJECT_NAME} SHARED + src/easynav_routes_maps_manager/RoutesMapsManager.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + tf2_ros::tf2_ros + pluginlib::pluginlib + ament_index_cpp::ament_index_cpp + yaets::yaets + yaml-cpp + ${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 plugin +pluginlib_export_plugin_description_file(easynav_core easynav_routes_maps_manager_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + tf2_ros + pluginlib + nav_msgs + ament_index_cpp + std_srvs + geometry_msgs + visualization_msgs + 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..827598e8 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -0,0 +1,54 @@ +# easynav_routes_maps_manager + +## Description + + +## Authors and Maintainers + +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Francisco Martín Rico + +## Supported ROS 2 Distributions + +| Distribution | Status | +|---|---| +| 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:** Routes (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_routes_maps_manager/RoutesMapsManager/...`) + +| Name | Type | Default | Description | +|---|---|---:|---| + +## Interfaces (Topics and Services) + +### Subscriptions and Publications + +| Direction | Topic | Type | Purpose | QoS | +|---|---|---|---|---| + +### Services + +| Direction | Service | Type | Purpose | +|---|---|---|---| + +## NavState Keys + +| Key | Type | Access | Notes | +|---|---|---|---| + +## TF Frames + + +## License + +GPL-3.0-only 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/RoutesMapsManager.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp new file mode 100644 index 00000000..6cd2a3e4 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp @@ -0,0 +1,110 @@ +// 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 method. + +#ifndef EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ +#define EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ + +#include "geometry_msgs/msg/pose.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "std_srvs/srv/trigger.hpp" + +#include "easynav_core/MapsManagerBase.hpp" + +namespace easynav +{ + +struct RouteSegment +{ + geometry_msgs::msg::Pose start; + geometry_msgs::msg::Pose end; +}; + +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. + * + * @return std::expected Success or error string. + */ + virtual std::expected 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 YAML file into routes_. + void load_routes_from_yaml(); + + /// @brief Publish the current routes as visualization markers. + void publish_routes_markers(); + /** + * @brief Full path to the map file. + */ + std::string map_path_; + /// @brief In-memory collection of route segments. + RoutesMap routes_; + + /// @brief Publisher for visualizing routes as markers. + rclcpp::Publisher::SharedPtr routes_pub_; + + /// @brief Service for reloading the routes YAML file. + rclcpp::Service::SharedPtr reload_routes_srv_; +}; + +} // namespace easynav + +#endif // EASYNAV_PLANNER__ROUTESMAPMANAGER_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..66777be3 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -0,0 +1,33 @@ + + + + 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 + tf2_ros + pluginlib + nav_msgs + ament_index_cpp + std_srvs + geometry_msgs + visualization_msgs + 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..bf7b86ef --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp @@ -0,0 +1,181 @@ +#include "easynav_routes_maps_manager/RoutesMapsManager.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(); + }); +} + +RoutesMapsManager::~RoutesMapsManager() = default; + +std::expected 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); + + map_path_.clear(); + 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 { + return std::unexpected( + "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::SystemDefaultsQoS()); + + reload_routes_srv_ = node->create_service( + node->get_fully_qualified_name() + std::string("/") + plugin_name + "/reload_routes", + [this](const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr response) { + try { + load_routes_from_yaml(); + publish_routes_markers(); + response->success = true; + response->message = "Routes reloaded"; + } catch (const std::exception & e) { + response->success = false; + response->message = e.what(); + } + }); + + try { + load_routes_from_yaml(); + publish_routes_markers(); + } catch (const std::exception & e) { + return std::unexpected(std::string{"Failed to load routes: "} + e.what()); + } + + return {}; +} + +void RoutesMapsManager::update(NavState & nav_state) +{ + // Expose routes map through NavState for other modules. + nav_state.set("routes", routes_); +} + +void RoutesMapsManager::load_routes_from_yaml() +{ + routes_.clear(); + + if (map_path_.empty()) { + throw std::runtime_error{"Parameter 'routes_map_path' is empty"}; + } + + YAML::Node root = YAML::LoadFile(map_path_); + + if (!root["routes"]) { + throw std::runtime_error{"YAML file must contain a 'routes' sequence"}; + } + + for (const auto & route_node : root["routes"]) { + if (!route_node["start"] || !route_node["end"]) { + continue; + } + + RouteSegment segment; + + 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); + } +} + +void RoutesMapsManager::publish_routes_markers() +{ + if (!routes_pub_) { + return; + } + + visualization_msgs::msg::MarkerArray array; + + int id = 0; + for (const auto & seg : routes_) { + visualization_msgs::msg::Marker line; + line.header.frame_id = "map"; + line.ns = "routes"; + 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); + } + + routes_pub_->publish(array); +} + +} // namespace easynav 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..9a8c4194 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -0,0 +1,11 @@ +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 + easynav_core::easynav_core +) 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..8c092467 --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -0,0 +1,156 @@ +#include +#include + +#include "easynav_routes_maps_manager/RoutesMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using easynav::RoutesMapsManager; +using easynav::RoutesMap; +using easynav::RouteSegment; + +class RoutesMapsManagerTestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(RoutesMapsManagerTestNode, LoadRoutesFromYamlAndPublish) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node"); + + // Create a temporary YAML file with two simple segments. + char filename[] = "/tmp/routes_test_XXXXXX.yaml"; + int fd = mkstemps(filename, 5); // keep .yaml suffix + ASSERT_NE(fd, -1); + close(fd); + + std::ofstream out(filename); + out << "routes:\n"; + out << " - start:\n"; + out << " x: 0.0\n"; + out << " y: 0.0\n"; + out << " z: 0.0\n"; + out << " qx: 0.0\n"; + out << " qy: 0.0\n"; + out << " qz: 0.0\n"; + out << " qw: 1.0\n"; + out << " end:\n"; + out << " x: 1.0\n"; + out << " y: 0.0\n"; + out << " z: 0.0\n"; + out << " qx: 0.0\n"; + out << " qy: 0.0\n"; + out << " qz: 0.0\n"; + out << " qw: 1.0\n"; + out << " - start:\n"; + out << " x: 1.0\n"; + out << " y: 1.0\n"; + out << " z: 0.0\n"; + out << " qx: 0.0\n"; + out << " qy: 0.0\n"; + out << " qz: 0.0\n"; + out << " qw: 1.0\n"; + out << " end:\n"; + out << " x: 2.0\n"; + out << " y: 1.0\n"; + out << " z: 0.0\n"; + out << " qx: 0.0\n"; + out << " qy: 0.0\n"; + out << " qz: 0.0\n"; + out << " qw: 1.0\n"; + out.close(); + + // Declare parameters before setting them to avoid exceptions. + node->declare_parameter("routes_maps_manager.package", std::string("")); + node->declare_parameter("routes_maps_manager.map_path_file", std::string("")); + + // Override parameters directly on the node. + node->set_parameters({ + rclcpp::Parameter("routes_maps_manager.map_path_file", std::string(filename)), + rclcpp::Parameter("routes_maps_manager.package", std::string("")) + }); + + auto manager = std::make_shared(); + auto init_result = manager->initialize(node, "routes_maps_manager"); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); + + // Activate lifecycle node before plugin-specific initialization. + auto state = node->configure(); + ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + state = node->activate(); + ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto result = manager->on_initialize(); + ASSERT_TRUE(result.has_value()) << result.error(); + + 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(RoutesMapsManagerTestNode, WritesRoutesIntoNavState) +{ + auto node = std::make_shared( + "routes_mapsmanager_test_node2"); + + // Use an empty but valid YAML file with no routes. + char filename[] = "/tmp/routes_test_empty_XXXXXX.yaml"; + int fd = mkstemps(filename, 5); + ASSERT_NE(fd, -1); + close(fd); + + std::ofstream out(filename); + out << "routes: []\n"; + out.close(); + + // Declare parameters before setting them to avoid exceptions. + node->declare_parameter("routes_maps_manager.package", std::string("")); + node->declare_parameter("routes_maps_manager.map_path_file", std::string("")); + + node->set_parameters({ + rclcpp::Parameter("routes_maps_manager.map_path_file", std::string(filename)), + rclcpp::Parameter("routes_maps_manager.package", std::string("")) + }); + + auto manager = std::make_shared(); + auto init_result = manager->initialize(node, "routes_maps_manager"); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); + + // Activate lifecycle node before plugin-specific initialization. + auto state = node->configure(); + ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + state = node->activate(); + ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto result = manager->on_initialize(); + ASSERT_TRUE(result.has_value()) << result.error(); + + easynav::NavState nav_state; + manager->update(nav_state); + + ASSERT_TRUE(nav_state.has("routes")); + const auto & routes = nav_state.get("routes"); + EXPECT_EQ(routes.size(), 0u); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From db81999e5ad6c48aaa39b44c5be57764b34dab0f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 30 Nov 2025 09:02:19 +0100 Subject: [PATCH 088/136] Interactive markers and ids in the segments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../RoutesMapsManager.hpp | 23 ++- .../RoutesMapsManager.cpp | 145 +++++++++++++++++- .../tests/routes_mapsmanager_tests.cpp | 40 +---- 3 files changed, 167 insertions(+), 41 deletions(-) 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 index 6cd2a3e4..5799a936 100644 --- 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 @@ -25,6 +25,8 @@ #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 "std_srvs/srv/trigger.hpp" @@ -35,6 +37,9 @@ namespace easynav struct RouteSegment { + /// @brief Unique identifier for this segment. + std::string id; + geometry_msgs::msg::Pose start; geometry_msgs::msg::Pose end; }; @@ -91,6 +96,13 @@ class RoutesMapsManager : public easynav::MapsManagerBase /// @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 routes_. + void handle_interactive_feedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); /** * @brief Full path to the map file. */ @@ -101,8 +113,15 @@ class RoutesMapsManager : public easynav::MapsManagerBase /// @brief Publisher for visualizing routes as markers. rclcpp::Publisher::SharedPtr routes_pub_; - /// @brief Service for reloading the routes YAML file. - rclcpp::Service::SharedPtr reload_routes_srv_; + /// @brief Publisher for interactive markers. + rclcpp::Publisher::SharedPtr imarker_pub_; + + /// @brief Subscription for interactive marker feedback. + rclcpp::Subscription::SharedPtr + imarker_feedback_sub_; + + /// @brief Service for saving current routes back to disk. + rclcpp::Service::SharedPtr save_routes_srv_; }; } // namespace easynav 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 index bf7b86ef..42176876 100644 --- 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 @@ -1,6 +1,7 @@ #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" #include +#include #include @@ -62,15 +63,86 @@ std::expected RoutesMapsManager::on_initialize() node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes", rclcpp::SystemDefaultsQoS()); - reload_routes_srv_ = node->create_service( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/reload_routes", + imarker_pub_ = node->create_publisher( + node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes_imarkers", + rclcpp::SystemDefaultsQoS()); + + imarker_feedback_sub_ = node->create_subscription< + visualization_msgs::msg::InteractiveMarkerFeedback>( + node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes_imarkers/feedback", + rclcpp::SystemDefaultsQoS(), + [this](const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback) { + handle_interactive_feedback(feedback); + }); + + 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 { - load_routes_from_yaml(); - publish_routes_markers(); + // 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 reloaded"; + response->message = "Routes saved"; } catch (const std::exception & e) { response->success = false; response->message = e.what(); @@ -80,6 +152,7 @@ std::expected RoutesMapsManager::on_initialize() try { load_routes_from_yaml(); publish_routes_markers(); + publish_interactive_markers(); } catch (const std::exception & e) { return std::unexpected(std::string{"Failed to load routes: "} + e.what()); } @@ -107,12 +180,22 @@ void RoutesMapsManager::load_routes_from_yaml() throw std::runtime_error{"YAML file must contain a 'routes' sequence"}; } - for (const auto & route_node : root["routes"]) { + // 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"]; @@ -178,4 +261,54 @@ void RoutesMapsManager::publish_routes_markers() routes_pub_->publish(array); } +void RoutesMapsManager::publish_interactive_markers() +{ + if (!imarker_pub_) { + return; + } + + int id = 0; + for (const auto & seg : routes_) { + // Start marker + visualization_msgs::msg::InteractiveMarker start_marker; + start_marker.header.frame_id = "map"; + start_marker.name = seg.id + "_start"; + start_marker.description = "Route " + seg.id + " start"; + start_marker.pose = seg.start; + + visualization_msgs::msg::InteractiveMarker end_marker; + end_marker.header.frame_id = "map"; + end_marker.name = seg.id + "_end"; + end_marker.description = "Route " + seg.id + " end"; + end_marker.pose = seg.end; + + imarker_pub_->publish(start_marker); + imarker_pub_->publish(end_marker); + + (void)id; + } +} + +void RoutesMapsManager::handle_interactive_feedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) +{ + if (!feedback) { + return; + } + + const auto & name = feedback->marker_name; + + for (auto & seg : routes_) { + if (name == seg.id + "_start") { + seg.start = feedback->pose; + publish_routes_markers(); + return; + } else if (name == seg.id + "_end") { + seg.end = feedback->pose; + publish_routes_markers(); + return; + } + } +} + } // namespace easynav 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 index 8c092467..2801a6b8 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -37,39 +37,13 @@ TEST_F(RoutesMapsManagerTestNode, LoadRoutesFromYamlAndPublish) close(fd); std::ofstream out(filename); - out << "routes:\n"; - out << " - start:\n"; - out << " x: 0.0\n"; - out << " y: 0.0\n"; - out << " z: 0.0\n"; - out << " qx: 0.0\n"; - out << " qy: 0.0\n"; - out << " qz: 0.0\n"; - out << " qw: 1.0\n"; - out << " end:\n"; - out << " x: 1.0\n"; - out << " y: 0.0\n"; - out << " z: 0.0\n"; - out << " qx: 0.0\n"; - out << " qy: 0.0\n"; - out << " qz: 0.0\n"; - out << " qw: 1.0\n"; - out << " - start:\n"; - out << " x: 1.0\n"; - out << " y: 1.0\n"; - out << " z: 0.0\n"; - out << " qx: 0.0\n"; - out << " qy: 0.0\n"; - out << " qz: 0.0\n"; - out << " qw: 1.0\n"; - out << " end:\n"; - out << " x: 2.0\n"; - out << " y: 1.0\n"; - out << " z: 0.0\n"; - out << " qx: 0.0\n"; - out << " qy: 0.0\n"; - out << " qz: 0.0\n"; - out << " qw: 1.0\n"; + out << "routes: [route1, route2]\n"; + out << "route1:\n"; + out << " start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + out << " end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + out << "route2:\n"; + out << " start: {x: 1.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + out << " end: {x: 2.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; out.close(); // Declare parameters before setting them to avoid exceptions. From a567212325d6f7a1730fe03c571072f689d8a00a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 1 Dec 2025 07:38:51 +0100 Subject: [PATCH 089/136] Add/Remove segment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CMakeLists.txt | 3 + .../RoutesMapsManager.hpp | 10 +- .../easynav_routes_maps_manager/package.xml | 1 + .../RoutesMapsManager.cpp | 243 ++++++++++++++++-- 4 files changed, 229 insertions(+), 28 deletions(-) diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt index f848cc1e..df3828c4 100644 --- a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt @@ -20,6 +20,7 @@ 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 @@ -37,6 +38,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ament_index_cpp::ament_index_cpp yaets::yaets yaml-cpp + interactive_markers::interactive_markers ${geometry_msgs_TARGETS} ${visualization_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -82,6 +84,7 @@ ament_export_dependencies( std_srvs geometry_msgs visualization_msgs + interactive_markers yaets yaml-cpp ) 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 index 5799a936..cec0d571 100644 --- 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 @@ -28,6 +28,8 @@ #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 "easynav_core/MapsManagerBase.hpp" @@ -113,12 +115,8 @@ class RoutesMapsManager : public easynav::MapsManagerBase /// @brief Publisher for visualizing routes as markers. rclcpp::Publisher::SharedPtr routes_pub_; - /// @brief Publisher for interactive markers. - rclcpp::Publisher::SharedPtr imarker_pub_; - - /// @brief Subscription for interactive marker feedback. - rclcpp::Subscription::SharedPtr - imarker_feedback_sub_; + /// @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_; diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index 66777be3..da17b4b3 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -20,6 +20,7 @@ std_srvs geometry_msgs visualization_msgs + interactive_markers yaets yaml-cpp 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 index 42176876..7d8c428c 100644 --- 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 @@ -61,19 +61,10 @@ std::expected RoutesMapsManager::on_initialize() routes_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes", - rclcpp::SystemDefaultsQoS()); - - imarker_pub_ = node->create_publisher( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes_imarkers", - rclcpp::SystemDefaultsQoS()); - - imarker_feedback_sub_ = node->create_subscription< - visualization_msgs::msg::InteractiveMarkerFeedback>( - node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes_imarkers/feedback", - rclcpp::SystemDefaultsQoS(), - [this](const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback) { - handle_interactive_feedback(feedback); - }); + 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", @@ -230,11 +221,26 @@ void RoutesMapsManager::publish_routes_markers() 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 = "map"; + 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 = "map"; - line.ns = "routes"; + line.ns = "routes_line"; line.id = id++; line.type = visualization_msgs::msg::Marker::LINE_LIST; line.action = visualization_msgs::msg::Marker::ADD; @@ -256,6 +262,40 @@ void RoutesMapsManager::publish_routes_markers() 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 = "map"; + 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 = 1.0; // shaft length + start_arrow.scale.y = 0.1; // shaft diameter + start_arrow.scale.z = 0.2; // 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 = "map"; + 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 = 1.0; + end_arrow.scale.y = 0.1; + end_arrow.scale.z = 0.2; + 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); @@ -263,30 +303,123 @@ void RoutesMapsManager::publish_routes_markers() void RoutesMapsManager::publish_interactive_markers() { - if (!imarker_pub_) { + if (!imarker_server_) { return; } + imarker_server_->clear(); - int id = 0; for (const auto & seg : routes_) { - // Start marker visualization_msgs::msg::InteractiveMarker start_marker; start_marker.header.frame_id = "map"; 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 = "map"; end_marker.name = seg.id + "_end"; end_marker.description = "Route " + seg.id + " end"; end_marker.pose = seg.end; - - imarker_pub_->publish(start_marker); - imarker_pub_->publish(end_marker); - - (void)id; + 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; + add_marker.scale.x = 0.4; + add_marker.scale.y = 0.4; + add_marker.scale.z = 0.4; + add_marker.color.r = 1.0f; + add_marker.color.g = 0.5f; + add_marker.color.b = 0.0f; + add_marker.color.a = 0.9f; + add_ctrl.markers.push_back(add_marker); + + 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.3; + remove_marker.scale.y = 0.3; + remove_marker.scale.z = 0.3; + remove_marker.color.r = 1.0f; + remove_marker.color.g = 0.0f; + remove_marker.color.b = 0.0f; + remove_marker.color.a = 0.9f; + remove_ctrl.markers.push_back(remove_marker); + + 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( @@ -298,6 +431,69 @@ void RoutesMapsManager::handle_interactive_feedback( const auto & name = feedback->marker_name; + // 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 segment id based on size + new_seg.id = "route" + std::to_string(routes_.size()); + + 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; @@ -312,3 +508,6 @@ void RoutesMapsManager::handle_interactive_feedback( } } // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::RoutesMapsManager, easynav::MapsManagerBase) From 60f302b22936441e7000c75d6cd92713d4678f2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 1 Dec 2025 08:18:59 +0100 Subject: [PATCH 090/136] Routes Maps manager finalized MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../RoutesMapsManager.hpp | 6 + .../RoutesMapsManager.cpp | 158 ++++++++++++++++-- 2 files changed, 150 insertions(+), 14 deletions(-) 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 index cec0d571..44346866 100644 --- 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 @@ -44,6 +44,9 @@ struct RouteSegment geometry_msgs::msg::Pose start; geometry_msgs::msg::Pose end; + + /// @brief Whether this segment is currently in edit mode. + bool edit_mode{false}; }; using RoutesMap = std::vector; @@ -112,6 +115,9 @@ class RoutesMapsManager : public easynav::MapsManagerBase /// @brief In-memory collection of route segments. RoutesMap routes_; + /// @brief Monotonic counter for generating unique route IDs. + int next_route_id_{0}; + /// @brief Publisher for visualizing routes as markers. rclcpp::Publisher::SharedPtr routes_pub_; 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 index 7d8c428c..caaf8b0c 100644 --- 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 @@ -211,6 +211,22 @@ void RoutesMapsManager::load_routes_from_yaml() 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() @@ -271,9 +287,9 @@ void RoutesMapsManager::publish_routes_markers() 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 = 1.0; // shaft length - start_arrow.scale.y = 0.1; // shaft diameter - start_arrow.scale.z = 0.2; // head diameter + 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; @@ -288,9 +304,9 @@ void RoutesMapsManager::publish_routes_markers() 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 = 1.0; - end_arrow.scale.y = 0.1; - end_arrow.scale.z = 0.2; + 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; @@ -309,6 +325,69 @@ void RoutesMapsManager::publish_interactive_markers() imarker_server_->clear(); 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 = "map"; + 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 = "map"; start_marker.name = seg.id + "_start"; @@ -367,14 +446,28 @@ void RoutesMapsManager::publish_interactive_markers() visualization_msgs::msg::Marker add_marker; add_marker.type = visualization_msgs::msg::Marker::SPHERE; - add_marker.scale.x = 0.4; - add_marker.scale.y = 0.4; - add_marker.scale.z = 0.4; + // 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); @@ -390,14 +483,27 @@ void RoutesMapsManager::publish_interactive_markers() // 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.3; - remove_marker.scale.y = 0.3; - remove_marker.scale.z = 0.3; + 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); }; @@ -431,6 +537,28 @@ void RoutesMapsManager::handle_interactive_feedback( 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 == @@ -455,8 +583,8 @@ void RoutesMapsManager::handle_interactive_feedback( const double length = 2.0; // meters RouteSegment new_seg; - // New segment id based on size - new_seg.id = "route" + std::to_string(routes_.size()); + // 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; @@ -498,10 +626,12 @@ void RoutesMapsManager::handle_interactive_feedback( 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; } } From c4914623ed79f852f5dda7b0edee4077b503000c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 2 Dec 2025 07:05:13 +0100 Subject: [PATCH 091/136] Route Maps Manager with filters. Costmap filter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CMakeLists.txt | 7 +- .../easynav_routes_filters_plugins.xml | 9 + .../RoutesFilter.hpp | 52 ++++++ .../RoutesMapsManager.hpp | 13 ++ .../filters/RoutesCostmapFilter.hpp | 40 ++++ .../RoutesMapsManager.cpp | 57 ++++++ .../filters/RoutesCostmapFilter.cpp | 171 ++++++++++++++++++ 7 files changed, 348 insertions(+), 1 deletion(-) create mode 100644 maps_managers/easynav_routes_maps_manager/easynav_routes_filters_plugins.xml create mode 100644 maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp create mode 100644 maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp create mode 100644 maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt index df3828c4..f817f364 100644 --- a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt @@ -12,6 +12,7 @@ set(CMAKE_CXX_EXTENSIONS OFF) 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) @@ -25,6 +26,7 @@ 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 $ @@ -33,6 +35,7 @@ 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 @@ -71,14 +74,16 @@ ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) -# Register the plugin +# 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 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/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..724b74aa --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp @@ -0,0 +1,52 @@ +// 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 + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace easynav +{ + +class NavState; + +class RoutesFilter +{ +public: + using Ptr = std::shared_ptr; + + virtual ~RoutesFilter() = default; + + virtual std::expected initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns, + const std::string & tf_prefix) = 0; + + 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 index 44346866..1660ba6c 100644 --- 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 @@ -32,8 +32,15 @@ #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 { @@ -118,6 +125,12 @@ class RoutesMapsManager : public easynav::MapsManagerBase /// @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_; 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..381c705a --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp @@ -0,0 +1,40 @@ +#ifndef EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ + +#include +#include + +#include "easynav_routes_maps_manager/RoutesFilter.hpp" + +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace easynav +{ + +class RoutesCostmapFilter : public RoutesFilter +{ +public: + RoutesCostmapFilter(); + + std::expected initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns, + const std::string & tf_prefix) override; + + void update(NavState & nav_state) override; + +private: + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + std::string plugin_ns_; + std::string tf_prefix_; + + int min_cost_{50}; + double route_width_{0.0}; // meters; 0.0 means default = one cell width + + rclcpp::Publisher::SharedPtr routes_occ_pub_; + 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/src/easynav_routes_maps_manager/RoutesMapsManager.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp index caaf8b0c..e8dc0944 100644 --- 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 @@ -26,6 +26,9 @@ RoutesMapsManager::RoutesMapsManager() } return out.str(); }); + + routes_filters_loader_ = std::make_unique>( + "easynav_routes_maps_manager", "easynav::RoutesFilter"); } RoutesMapsManager::~RoutesMapsManager() = default; @@ -46,6 +49,13 @@ std::expected RoutesMapsManager::on_initialize() 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 (!map_path_file.empty() && map_path_file[0] == '/') { // Absolute path: ignore package_name. @@ -148,6 +158,48 @@ std::expected RoutesMapsManager::on_initialize() return std::unexpected(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); + + auto result = instance->initialize(node, plugin_name + "." + filter_name, get_tf_prefix()); + if (!result) { + RCLCPP_ERROR(node->get_logger(), + "Unable to initialize RoutesFilter %s [%s]. Error: %s", + filter_name.c_str(), plugin.c_str(), result.error().c_str()); + return std::unexpected("Unable to initialize RoutesFilter " + plugin + + " . Error: " + result.error()); + } + + 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()); + return std::unexpected("Unable to load plugin easynav::RoutesFilter " + + filter_name + " . Error: " + ex.what()); + } + } + return {}; } @@ -155,6 +207,11 @@ 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() 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..0773bc7f --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp @@ -0,0 +1,171 @@ +#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" + +namespace easynav +{ + +RoutesCostmapFilter::RoutesCostmapFilter() = default; + +std::expected +RoutesCostmapFilter::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_ns, + const std::string & tf_prefix) +{ + node_ = node; + plugin_ns_ = plugin_ns; + tf_prefix_ = tf_prefix; + + // 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()); + + return {}; +} + +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; + if (c1 <= 0.0) { + continue; + } + const double c2 = vx * vx + vy * vy; + 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; + } + if (dist2 <= max_dist * max_dist) { + 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 = tf_prefix_ + "map"; + 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) From 91d4ca68bc168749c9bba60a2c5b21c9fe971710 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 2 Dec 2025 07:32:17 +0100 Subject: [PATCH 092/136] Tests and README MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_routes_maps_manager/README.md | 103 ++++++- .../RoutesFilter.hpp | 29 ++ .../RoutesMapsManager.hpp | 24 +- .../filters/RoutesCostmapFilter.hpp | 48 ++- .../RoutesMapsManager.cpp | 288 +++++++++++------- .../filters/RoutesCostmapFilter.cpp | 94 +++--- .../tests/CMakeLists.txt | 27 +- .../tests/routes_costmap_filter_tests.cpp | 139 +++++++++ .../tests/routes_mapsmanager_tests.cpp | 178 +++++++---- 9 files changed, 709 insertions(+), 221 deletions(-) create mode 100644 maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md index 827598e8..30bb391e 100644 --- a/maps_managers/easynav_routes_maps_manager/README.md +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -2,6 +2,18 @@ ## 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 @@ -12,6 +24,8 @@ | 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) @@ -20,7 +34,16 @@ - **Type:** `easynav::RoutesMapsManager` - **Base Class:** `easynav::MapsManagerBase` - **Library:** `easynav_routes_maps_manager` -- **Description:** Routes (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. +- **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 @@ -28,6 +51,75 @@ | 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) @@ -35,19 +127,28 @@ | 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 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 index 724b74aa..859744fc 100644 --- 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 @@ -32,18 +32,47 @@ 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_prefix TF frame prefix used by the navigation stack. + /// @return std::expected Empty on success or + /// an error message describing the failure. virtual std::expected initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns, const std::string & tf_prefix) = 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; }; 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 index 1660ba6c..c2f4705e 100644 --- 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 @@ -18,7 +18,7 @@ // along with this program. If not, see . /// \file -/// \brief Declaration of the RoutesMapsManager method. +/// \brief Declaration of the RoutesMapsManager class and related types. #ifndef EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ #define EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ @@ -44,18 +44,27 @@ 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; /** @@ -103,7 +112,11 @@ class RoutesMapsManager : public easynav::MapsManagerBase const RoutesMap & get_routes() const {return routes_;} private: - /// @brief Load routes from the YAML file into routes_. + /// @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. @@ -112,13 +125,12 @@ class RoutesMapsManager : public easynav::MapsManagerBase /// @brief Publish or update interactive markers for editing endpoints. void publish_interactive_markers(); - /// @brief Handle feedback from interactive markers and update routes_. + /// @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 map file. - */ + /// @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_; 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 index 381c705a..fe23fe16 100644 --- 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 @@ -11,27 +11,73 @@ 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_prefix TF frame prefix; used to set the frame id of + /// the published debug grid. + /// @return std::expected Empty on success or an + /// error message on failure. std::expected initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns, const std::string & tf_prefix) 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 TF prefix used for published frame ids. std::string tf_prefix_; + /// @brief Minimum cost enforced outside the route corridor. int min_cost_{50}; - double route_width_{0.0}; // meters; 0.0 means default = one cell width + /// @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_; }; 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 index e8dc0944..fb3005aa 100644 --- 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 @@ -15,16 +15,16 @@ 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 << ", " + [](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(); + } + return out.str(); }); routes_filters_loader_ = std::make_unique>( @@ -57,7 +57,13 @@ std::expected RoutesMapsManager::on_initialize() node->get_parameter(plugin_name + ".filters", routes_filters_names); map_path_.clear(); - if (!map_path_file.empty() && map_path_file[0] == '/') { + 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()) { @@ -219,13 +225,81 @@ void RoutesMapsManager::load_routes_from_yaml() routes_.clear(); if (map_path_.empty()) { - throw std::runtime_error{"Parameter 'routes_map_path' is 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 = YAML::LoadFile(map_path_); + 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"]) { - throw std::runtime_error{"YAML file must contain a 'routes' sequence"}; + // 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, ...] @@ -460,110 +534,110 @@ void RoutesMapsManager::publish_interactive_markers() end_marker.scale = 1.0; auto add_controls = [](visualization_msgs::msg::InteractiveMarker & marker) { - visualization_msgs::msg::InteractiveMarkerControl control; + 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); + 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); + 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); + 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); + 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; + 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; + 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 + 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); + add_ctrl.markers.push_back(add_marker); + add_ctrl.markers.push_back(add_text); - marker.controls.push_back(add_ctrl); + 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; + 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; + 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); - }; + 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); @@ -596,10 +670,10 @@ void RoutesMapsManager::handle_interactive_feedback( // 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)) + (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("_"); @@ -618,10 +692,10 @@ void RoutesMapsManager::handle_interactive_feedback( // 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)) + (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; @@ -658,10 +732,10 @@ void RoutesMapsManager::handle_interactive_feedback( // 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)) + (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("_"); 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 index 0773bc7f..2a233cfa 100644 --- 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 @@ -81,7 +81,8 @@ RoutesCostmapFilter::update(NavState & nav_state) const unsigned int size_y = costmap.getSizeInCellsY(); // Precompute a list of segments in world coordinates - struct Segment2D { + struct Segment2D + { double x0, y0, x1, y1; }; @@ -96,52 +97,55 @@ RoutesCostmapFilter::update(NavState & nav_state) } 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; - if (c1 <= 0.0) { - continue; - } - const double c2 = vx * vx + vy * vy; - if (c2 <= 0.0) { - continue; - } - const double t = c1 / c2; - if (t < 0.0 || t > 1.0) { - continue; + 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; + } } - 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; - } - if (dist2 <= max_dist * max_dist) { - return true; - } - } - - return false; - }; + return false; + }; for (unsigned int y = 0; y < size_y; ++y) { for (unsigned int x = 0; x < size_x; ++x) { diff --git a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt index 9a8c4194..b185f48c 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -1,11 +1,26 @@ +cmake_minimum_required(VERSION 3.5) + find_package(ament_cmake_gtest REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_costmap_common REQUIRED) +find_package(easynav_routes_maps_manager REQUIRED) ament_add_gtest(routes_mapsmanager_tests - routes_mapsmanager_tests.cpp -) - + routes_mapsmanager_tests.cpp) target_link_libraries(routes_mapsmanager_tests - ${PROJECT_NAME} + easynav_routes_maps_manager::easynav_routes_maps_manager easynav_common::easynav_common - easynav_core::easynav_core -) + 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 + easynav_routes_maps_manager::easynav_routes_maps_manager + 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..dbf2c1dd --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp @@ -0,0 +1,139 @@ +#include + +#include "easynav_common/types/NavState.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; + auto init_result = filter.initialize(node, "routes.routes_costmap", ""); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); + + 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; + auto init_result = filter.initialize(node, "routes.routes_costmap", ""); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); + + // 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; + auto init_result = filter.initialize(node, "routes.routes_costmap", ""); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); + + // 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 index 2801a6b8..a11a60e7 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -1,17 +1,20 @@ +// Tests for RoutesMapsManager + #include #include +#include "easynav_common/types/NavState.hpp" + #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "lifecycle_msgs/msg/state.hpp" using easynav::RoutesMapsManager; using easynav::RoutesMap; using easynav::RouteSegment; -class RoutesMapsManagerTestNode : public ::testing::Test +class RoutesMapsManagerTest : public ::testing::Test { protected: static void SetUpTestCase() @@ -25,50 +28,51 @@ class RoutesMapsManagerTestNode : public ::testing::Test } }; -TEST_F(RoutesMapsManagerTestNode, LoadRoutesFromYamlAndPublish) +// Helper to create a temporary YAML file with given contents +static std::string create_temp_yaml(const std::string & contents) { - auto node = std::make_shared( - "routes_mapsmanager_test_node"); - - // Create a temporary YAML file with two simple segments. char filename[] = "/tmp/routes_test_XXXXXX.yaml"; int fd = mkstemps(filename, 5); // keep .yaml suffix - ASSERT_NE(fd, -1); + if (fd == -1) { + throw std::runtime_error("Unable to create temp file"); + } close(fd); std::ofstream out(filename); - out << "routes: [route1, route2]\n"; - out << "route1:\n"; - out << " start: {x: 0.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; - out << " end: {x: 1.0, y: 0.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; - out << "route2:\n"; - out << " start: {x: 1.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; - out << " end: {x: 2.0, y: 1.0, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0}\n"; + 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"; - // Declare parameters before setting them to avoid exceptions. - node->declare_parameter("routes_maps_manager.package", std::string("")); - node->declare_parameter("routes_maps_manager.map_path_file", std::string("")); + 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("")); - // Override parameters directly on the node. node->set_parameters({ - rclcpp::Parameter("routes_maps_manager.map_path_file", std::string(filename)), - rclcpp::Parameter("routes_maps_manager.package", std::string("")) + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) }); auto manager = std::make_shared(); - auto init_result = manager->initialize(node, "routes_maps_manager"); + auto init_result = manager->initialize(node, "routes"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); - // Activate lifecycle node before plugin-specific initialization. - auto state = node->configure(); - ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - state = node->activate(); - ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - auto result = manager->on_initialize(); - ASSERT_TRUE(result.has_value()) << result.error(); - const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 2u); @@ -78,49 +82,113 @@ TEST_F(RoutesMapsManagerTestNode, LoadRoutesFromYamlAndPublish) EXPECT_DOUBLE_EQ(routes[1].end.position.x, 2.0); } -TEST_F(RoutesMapsManagerTestNode, WritesRoutesIntoNavState) +TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) { auto node = std::make_shared( - "routes_mapsmanager_test_node2"); + "routes_mapsmanager_test_node_empty_path"); - // Use an empty but valid YAML file with no routes. - char filename[] = "/tmp/routes_test_empty_XXXXXX.yaml"; - int fd = mkstemps(filename, 5); - ASSERT_NE(fd, -1); - close(fd); + node->declare_parameter("routes.package", std::string("")); + node->declare_parameter("routes.map_path_file", std::string("")); - std::ofstream out(filename); - out << "routes: []\n"; - out.close(); + node->set_parameters({ + rclcpp::Parameter("routes.map_path_file", std::string("")), + rclcpp::Parameter("routes.package", std::string("")) + }); + + auto manager = std::make_shared(); + auto init_result = manager->initialize(node, "routes"); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); + + 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")); - // Declare parameters before setting them to avoid exceptions. - node->declare_parameter("routes_maps_manager.package", std::string("")); - node->declare_parameter("routes_maps_manager.map_path_file", std::string("")); + 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(); + auto init_result = manager->initialize(node, "routes"); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); + + 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_maps_manager.map_path_file", std::string(filename)), - rclcpp::Parameter("routes_maps_manager.package", std::string("")) + rclcpp::Parameter("routes.map_path_file", filename), + rclcpp::Parameter("routes.package", std::string("")) }); auto manager = std::make_shared(); - auto init_result = manager->initialize(node, "routes_maps_manager"); + auto init_result = manager->initialize(node, "routes"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); - // Activate lifecycle node before plugin-specific initialization. - auto state = node->configure(); - ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - state = node->activate(); - ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + 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); - auto result = manager->on_initialize(); - ASSERT_TRUE(result.has_value()) << result.error(); + 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(); + auto init_result = manager->initialize(node, "routes"); + ASSERT_TRUE(init_result.has_value()) << init_result.error(); easynav::NavState nav_state; manager->update(nav_state); ASSERT_TRUE(nav_state.has("routes")); const auto & routes = nav_state.get("routes"); - EXPECT_EQ(routes.size(), 0u); + 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) From 84af6b25e8340921a1f6355831b6875d3ffa5a02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 2 Dec 2025 12:17:59 +0100 Subject: [PATCH 093/136] Added missing licenses MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../filters/RoutesCostmapFilter.hpp | 19 ++++++++++++++++++ .../RoutesMapsManager.cpp | 20 +++++++++++++++++++ .../filters/RoutesCostmapFilter.cpp | 20 +++++++++++++++++++ .../tests/routes_costmap_filter_tests.cpp | 19 ++++++++++++++++++ .../tests/routes_mapsmanager_tests.cpp | 20 ++++++++++++++++++- 5 files changed, 97 insertions(+), 1 deletion(-) 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 index fe23fe16..6baa4c6f 100644 --- 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 @@ -1,3 +1,22 @@ +// 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_ 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 index fb3005aa..f61cde44 100644 --- 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 @@ -1,3 +1,23 @@ +// 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 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 index 2a233cfa..90426bfd 100644 --- 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 @@ -1,3 +1,23 @@ +// 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 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 index dbf2c1dd..be9bbeaf 100644 --- 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 @@ -1,3 +1,22 @@ +// 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" 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 index a11a60e7..8e834c74 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -1,4 +1,22 @@ -// Tests for RoutesMapsManager +// 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 From c28e3c9c238480185e728f024f4c66229ad37b1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 2 Dec 2025 12:36:49 +0100 Subject: [PATCH 094/136] Added missing dep MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- maps_managers/easynav_routes_maps_manager/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/maps_managers/easynav_routes_maps_manager/package.xml b/maps_managers/easynav_routes_maps_manager/package.xml index da17b4b3..2bcb255a 100644 --- a/maps_managers/easynav_routes_maps_manager/package.xml +++ b/maps_managers/easynav_routes_maps_manager/package.xml @@ -13,6 +13,7 @@ rclcpp_lifecycle --> easynav_common easynav_core + easynav_costmap_common tf2_ros pluginlib nav_msgs From 6f19f31f62b3539834cf610366fbed3e66a3217c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 2 Dec 2025 12:40:52 +0100 Subject: [PATCH 095/136] Fix CMakeLists MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_routes_maps_manager/tests/CMakeLists.txt | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt index b185f48c..3638b4d4 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -1,16 +1,9 @@ -cmake_minimum_required(VERSION 3.5) - find_package(ament_cmake_gtest REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(easynav_common REQUIRED) -find_package(easynav_costmap_common REQUIRED) -find_package(easynav_routes_maps_manager REQUIRED) ament_add_gtest(routes_mapsmanager_tests routes_mapsmanager_tests.cpp) target_link_libraries(routes_mapsmanager_tests - easynav_routes_maps_manager::easynav_routes_maps_manager + ${PROJECT_NAME} easynav_common::easynav_common rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle) @@ -18,7 +11,7 @@ target_link_libraries(routes_mapsmanager_tests ament_add_gtest(routes_costmap_filter_tests routes_costmap_filter_tests.cpp) target_link_libraries(routes_costmap_filter_tests - easynav_routes_maps_manager::easynav_routes_maps_manager + ${PROJECT_NAME} easynav_common::easynav_common easynav_costmap_common::easynav_costmap_common rclcpp::rclcpp From 149636226a53acefbb063bb6151b89178202dc66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 2 Dec 2025 12:49:34 +0100 Subject: [PATCH 096/136] linting MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_planner/CostmapPlanner.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 b1b88b4e..59461fb0 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -148,8 +148,11 @@ void CostmapPlanner::update(NavState & nav_state) 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); + 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 && From 9296d2ff69ba4868a6a5540d18ca8f3984fdd0f4 Mon Sep 17 00:00:00 2001 From: migueldm Date: Wed, 26 Nov 2025 12:32:36 +0100 Subject: [PATCH 097/136] First working version --- .../easynav_fusion_localizer/CHANGELOG.rst | 8 + .../easynav_fusion_localizer/CMakeLists.txt | 96 + localizers/easynav_fusion_localizer/README.md | 54 + .../config/example_params.yaml | 133 + .../easynav_fusion_localizer_plugins.xml | 9 + .../FusionLocalizer.hpp | 78 + .../easynav_fusion_localizer/ukf_wrapper.hpp | 891 ++++ .../easynav_fusion_localizer/package.xml | 32 + .../FusionLocalizer.cpp | 175 + .../easynav_fusion_localizer/ukf_wrapper.cpp | 3765 +++++++++++++++++ 10 files changed, 5241 insertions(+) create mode 100644 localizers/easynav_fusion_localizer/CHANGELOG.rst create mode 100644 localizers/easynav_fusion_localizer/CMakeLists.txt create mode 100644 localizers/easynav_fusion_localizer/README.md create mode 100644 localizers/easynav_fusion_localizer/config/example_params.yaml create mode 100644 localizers/easynav_fusion_localizer/easynav_fusion_localizer_plugins.xml create mode 100644 localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp create mode 100644 localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp create mode 100644 localizers/easynav_fusion_localizer/package.xml create mode 100644 localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp create mode 100644 localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp 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..23a79bd0 --- /dev/null +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -0,0 +1,96 @@ +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() + +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) +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..4c6edf64 --- /dev/null +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -0,0 +1,78 @@ +#pragma once + +#include +#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. + * + * Crea e inicializa el UkfWrapper, que cargará parámetros + * y creará todos los suscriptores/publicadores de robot_localization. + */ + std::expected 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}; + + std::string base_link_frame_id_; + std::string odom_frame_id_; + std::string world_frame_id_; + + 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 \ No newline at end of file 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..ee2f2c76 --- /dev/null +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -0,0 +1,891 @@ +/* + * 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 "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_; + } + + std::string getBaseLinkFrameId() const + { + return base_link_frame_id_; + } + + std::string getWorldFrameId() const + { + return world_frame_id_; + } + + std::string getOdomFrameId() const + { + return odom_frame_id_; + } + +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..ed822341 --- /dev/null +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -0,0 +1,175 @@ +#include + +#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 +{ + +std::expected 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 std::string& tf_prefix = this->get_tf_prefix(); + RCLCPP_INFO(localizer_node->get_logger(), "Using tf_prefix: '%s'", 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_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()); + // Return an error to easynav + return std::unexpected(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()); + + base_link_frame_id_ = ukf_wrapper_->getBaseLinkFrameId(); + world_frame_id_ = ukf_wrapper_->getWorldFrameId(); + odom_frame_id_ = ukf_wrapper_->getOdomFrameId(); + + RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); + return {}; +} + +// 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; + std::cout << "FusionLocalizer: Processing GNSS sensor data " << i << std::endl; + auto pose = navsatfix_to_pose(gps_data[i]->data); + std::cout << "GPS UTM POSE (X, Y): " << pose.pose.pose.position.x << ", " << pose.pose.pose.position.y << std::endl; + // nav_state.set("UTM_gnss_pose", pose); + // Call the wrapper callback + ukf_wrapper_->poseCallback( + std::make_shared(pose), + ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data + ukf_wrapper_->getWorldFrameId(), // target_frame + ukf_wrapper_->getOdomFrameId(), // pose_source_frame + false // imu_data + ); + std::cout << "FusionLocalizer: Processed!!" << std::endl; + } + } + } + + std::cout << "Updating..." << std::endl; + ukf_wrapper_->periodicUpdate(); + std::cout << "Updated!!" << std::endl; + + 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; + + // 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 = world_frame_id_; + + // 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) \ No newline at end of file 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..f5b2aa62 --- /dev/null +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -0,0 +1,3765 @@ +/* + * 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 + +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_ + "."; + + 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()); + } + } + + // These params specify the name of the robot's body frame (typically + // base_link) and odometry frame (typically odom) + map_frame_id_ = parent_node_->declare_parameter(param_prefix + "map_frame", std::string("map")); + odom_frame_id_ = parent_node_->declare_parameter(param_prefix + "odom_frame", std::string("odom")); + base_link_frame_id_ = parent_node_->declare_parameter(param_prefix + + "base_link_frame", + std::string("base_link")); + base_link_output_frame_id_ = parent_node_->declare_parameter(param_prefix + + "base_link_frame_output", + 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 odom_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). + */ + world_frame_id_ = parent_node_->declare_parameter(param_prefix + "world_frame", odom_frame_id_); + + 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)); + + // 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)); + } 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)); + + 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)); + + 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)); + } 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)); + } else { + control_sub_ = parent_node_->create_subscription( + "cmd_vel", rclcpp::QoS(1), + std::bind(&UkfWrapper::controlCallback, this, std::placeholders::_1)); + } + } + + /* 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 From b49f3795023757fd6e29c6dc0b98147a32f3fb0f Mon Sep 17 00:00:00 2001 From: migueldm Date: Fri, 28 Nov 2025 10:50:12 +0100 Subject: [PATCH 098/136] added pkg to workflow and ament_uncrustify code --- .github/workflows/rolling_cron.yaml | 2 +- .../FusionLocalizer.hpp | 5 +- .../easynav_fusion_localizer/ukf_wrapper.hpp | 17 +-- .../FusionLocalizer.cpp | 54 ++++----- .../easynav_fusion_localizer/ukf_wrapper.cpp | 107 ++++++++++-------- 5 files changed, 102 insertions(+), 83 deletions(-) diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml index 40abdff2..39e4cd22 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.github/workflows/rolling_cron.yaml @@ -23,7 +23,7 @@ jobs: - 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 target-ros2-distro: rolling ref: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index 4c6edf64..e88eb605 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -50,7 +50,6 @@ class FusionLocalizer : public easynav::LocalizerMethodBase void update(NavState & nav_state) override; private: - std::unique_ptr ukf_wrapper_; int n_imu_sensors_{0}; @@ -59,7 +58,7 @@ class FusionLocalizer : public easynav::LocalizerMethodBase std::string base_link_frame_id_; std::string odom_frame_id_; std::string world_frame_id_; - + geometry_msgs::msg::PoseWithCovarianceStamped navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg); @@ -75,4 +74,4 @@ class FusionLocalizer : public easynav::LocalizerMethodBase }; -} // namespace easynav \ No newline at end of file +} // 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 index ee2f2c76..698f85cb 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -57,7 +57,7 @@ #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 "robot_localization/ukf.hpp" #include "sensor_msgs/msg/imu.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2/LinearMath/Transform.hpp" @@ -104,9 +104,10 @@ class UkfWrapper //! 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); + explicit UkfWrapper( + std::shared_ptr parent_node, + const std::string & tf_prefix, + const std::string & plugin_name); //! @brief Destructor //! @@ -342,22 +343,22 @@ class UkfWrapper //! bool validateFilterOutput(nav_msgs::msg::Odometry * message); - std::vector getGpsCallbackDataArr() const + std::vector getGpsCallbackDataArr() const { return gps_callbackData_arr_; } - std::string getBaseLinkFrameId() const + std::string getBaseLinkFrameId() const { return base_link_frame_id_; } - std::string getWorldFrameId() const + std::string getWorldFrameId() const { return world_frame_id_; } - std::string getOdomFrameId() const + std::string getOdomFrameId() const { return odom_frame_id_; } diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index ed822341..561170bb 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -18,32 +18,32 @@ namespace easynav std::expected 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 std::string& tf_prefix = this->get_tf_prefix(); + + const std::string & plugin_name = this->get_plugin_name(); + const std::string & tf_prefix = this->get_tf_prefix(); RCLCPP_INFO(localizer_node->get_logger(), "Using tf_prefix: '%s'", 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_prefix, plugin_name + ".local_filter" ); ukf_wrapper_->initialize(); - localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); + 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->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->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( @@ -57,7 +57,8 @@ std::expected FusionLocalizer::on_initialize() int zone; bool northp; - GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, zone, northp, UTM_origin_x_, UTM_origin_y_); + 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_; @@ -79,12 +80,13 @@ void FusionLocalizer::update_rt(NavState & nav_state) 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 (gps_time > last_gps_stamp_[i]) { // if(true) { last_gps_stamp_[i] = gps_time; std::cout << "FusionLocalizer: Processing GNSS sensor data " << i << std::endl; auto pose = navsatfix_to_pose(gps_data[i]->data); - std::cout << "GPS UTM POSE (X, Y): " << pose.pose.pose.position.x << ", " << pose.pose.pose.position.y << std::endl; + std::cout << "GPS UTM POSE (X, Y): " << pose.pose.pose.position.x << ", " << + pose.pose.pose.position.y << std::endl; // nav_state.set("UTM_gnss_pose", pose); // Call the wrapper callback ukf_wrapper_->poseCallback( @@ -98,7 +100,7 @@ void FusionLocalizer::update_rt(NavState & nav_state) } } } - + std::cout << "Updating..." << std::endl; ukf_wrapper_->periodicUpdate(); std::cout << "Updated!!" << std::endl; @@ -112,7 +114,7 @@ void FusionLocalizer::update_rt(NavState & nav_state) // 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( @@ -130,7 +132,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose double utm_x, utm_y; int zone; bool northp; - + GeographicLib::UTMUPS::Forward( navsat_msg.latitude, navsat_msg.longitude, @@ -138,7 +140,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose 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_; @@ -150,13 +152,13 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose 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[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[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 @@ -172,4 +174,4 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose } // namespace easynav #include -PLUGINLIB_EXPORT_CLASS(easynav::FusionLocalizer, easynav::LocalizerMethodBase) \ No newline at end of file +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 index f5b2aa62..81940439 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -81,7 +81,8 @@ namespace robot_localization { using namespace std::chrono_literals; -UkfWrapper::UkfWrapper(std::shared_ptr parent_node, +UkfWrapper::UkfWrapper( + std::shared_ptr parent_node, const std::string & tf_prefix, const std::string & plugin_name) : print_diagnostics_(true), @@ -822,7 +823,7 @@ void UkfWrapper::loadParams() 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); + 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 @@ -859,7 +860,8 @@ void UkfWrapper::loadParams() 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_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 @@ -881,11 +883,12 @@ void UkfWrapper::loadParams() // These params specify the name of the robot's body frame (typically // base_link) and odometry frame (typically odom) map_frame_id_ = parent_node_->declare_parameter(param_prefix + "map_frame", std::string("map")); - odom_frame_id_ = parent_node_->declare_parameter(param_prefix + "odom_frame", std::string("odom")); - base_link_frame_id_ = parent_node_->declare_parameter(param_prefix + + odom_frame_id_ = parent_node_->declare_parameter(param_prefix + "odom_frame", + std::string("odom")); + base_link_frame_id_ = parent_node_->declare_parameter(param_prefix + "base_link_frame", std::string("base_link")); - base_link_output_frame_id_ = parent_node_->declare_parameter(param_prefix + + base_link_output_frame_id_ = parent_node_->declare_parameter(param_prefix + "base_link_frame_output", base_link_frame_id_); @@ -942,10 +945,12 @@ void UkfWrapper::loadParams() 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); + 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); + 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); @@ -958,10 +963,12 @@ void UkfWrapper::loadParams() // 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); + 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_)); + 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 @@ -969,7 +976,8 @@ void UkfWrapper::loadParams() // 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); + 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( @@ -1021,7 +1029,8 @@ void UkfWrapper::loadParams() use_control_ = false; } - parent_node_->declare_parameter(param_prefix + "acceleration_limits", rclcpp::PARAMETER_DOUBLE_ARRAY); + 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( @@ -1039,7 +1048,8 @@ void UkfWrapper::loadParams() acceleration_limits.resize(TWIST_SIZE, 1.0); } - parent_node_->declare_parameter(param_prefix + "acceleration_gains", rclcpp::PARAMETER_DOUBLE_ARRAY); + 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) { @@ -1054,7 +1064,8 @@ void UkfWrapper::loadParams() } } - parent_node_->declare_parameter(param_prefix + "deceleration_limits", rclcpp::PARAMETER_DOUBLE_ARRAY); + 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( @@ -1071,7 +1082,8 @@ void UkfWrapper::loadParams() deceleration_limits = acceleration_limits; } - parent_node_->declare_parameter(param_prefix + "deceleration_gains", rclcpp::PARAMETER_DOUBLE_ARRAY); + 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) { @@ -1099,7 +1111,7 @@ void UkfWrapper::loadParams() deceleration_gains.resize(TWIST_SIZE, 1.0); } - bool dynamic_process_noise_covariance = parent_node_->declare_parameter(param_prefix + + bool dynamic_process_noise_covariance = parent_node_->declare_parameter(param_prefix + "dynamic_process_noise_covariance", false); filter_.setUseDynamicProcessNoiseCovariance( dynamic_process_noise_covariance); @@ -1120,7 +1132,8 @@ void UkfWrapper::loadParams() } // Check if the filter should start or not - disabled_at_startup_ = parent_node_->declare_parameter(param_prefix +"disabled_at_startup", false); + disabled_at_startup_ = parent_node_->declare_parameter(param_prefix + "disabled_at_startup", + false); enabled_ = !disabled_at_startup_; // Debugging writes to file @@ -1215,12 +1228,13 @@ void UkfWrapper::loadParams() if (more_params) { // Determine if we want to integrate this sensor differentially - bool differential = parent_node_->declare_parameter(param_prefix + + 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); + bool relative = parent_node_->declare_parameter(param_prefix + odom_topic_name + + std::string("_relative"), false); if (relative && differential) { RCLCPP_ERROR_STREAM( @@ -1232,23 +1246,23 @@ void UkfWrapper::loadParams() } // 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 + + 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 + + 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 + + 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 + + int queue_size = parent_node_->declare_parameter(param_prefix + odom_topic_name + std::string("_queue_size"), 10); @@ -1369,12 +1383,12 @@ void UkfWrapper::loadParams() } if (more_params) { - bool differential = parent_node_->declare_parameter(param_prefix + + 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 + + bool relative = parent_node_->declare_parameter(param_prefix + pose_topic_name + std::string("_relative"), false); @@ -1388,13 +1402,13 @@ void UkfWrapper::loadParams() } // Check for pose rejection threshold - double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + 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 + + int queue_size = parent_node_->declare_parameter(param_prefix + pose_topic_name + std::string("_queue_size"), 10); @@ -1487,12 +1501,12 @@ void UkfWrapper::loadParams() } if (more_params) { - bool differential = parent_node_->declare_parameter(param_prefix + + 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 + + bool relative = parent_node_->declare_parameter(param_prefix + gps_topic_name + std::string("_relative"), false); @@ -1506,13 +1520,13 @@ void UkfWrapper::loadParams() } // Check for gps rejection threshold - double gps_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + 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 + + int queue_size = parent_node_->declare_parameter(param_prefix + gps_topic_name + std::string("_queue_size"), 10); @@ -1610,13 +1624,13 @@ void UkfWrapper::loadParams() if (more_params) { // Check for twist rejection threshold - double twist_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + 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 + + int queue_size = parent_node_->declare_parameter(param_prefix + twist_topic_name + std::string("_queue_size"), 10); @@ -1689,12 +1703,13 @@ void UkfWrapper::loadParams() } if (more_params) { - bool differential = parent_node_->declare_parameter(param_prefix + + 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); + bool relative = parent_node_->declare_parameter(param_prefix + imu_topic_name + + std::string("_relative"), false); if (relative && differential) { RCLCPP_ERROR_STREAM( @@ -1706,7 +1721,7 @@ void UkfWrapper::loadParams() } // Check for pose rejection threshold - double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + imu_topic_name + std::string("_pose_rejection_threshold"), std::numeric_limits::max()); @@ -1714,17 +1729,17 @@ void UkfWrapper::loadParams() // 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 + + 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 + + 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 + + bool remove_grav_acc = parent_node_->declare_parameter(param_prefix + imu_topic_name + "_remove_gravitational_acceleration", false); @@ -1732,7 +1747,7 @@ void UkfWrapper::loadParams() remove_grav_acc; // Set optional custom queue size - int queue_size = parent_node_->declare_parameter(param_prefix + + int queue_size = parent_node_->declare_parameter(param_prefix + imu_topic_name + std::string("_queue_size"), 10); @@ -1997,8 +2012,8 @@ void UkfWrapper::loadParams() covariance.setZero(); std::vector covar_flat; - parent_node_->declare_parameter( parameter, rclcpp::PARAMETER_DOUBLE_ARRAY); - if (parent_node_->get_parameter( parameter, 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 " @@ -2018,12 +2033,14 @@ void UkfWrapper::loadParams() return false; }; - if (load_covariance(param_prefix +"process_noise_covariance", process_noise_covariance_)) { + 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_)) { + 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_); } @@ -2779,7 +2796,7 @@ std::vector UkfWrapper::loadUpdateConfig(const std::string & topic_name) 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); + update_vector = parent_node_->declare_parameter(topic_config_name, update_vector); return update_vector; } From c598c9cdc8dc5ee3d3e27695200953bcbaf9d39b Mon Sep 17 00:00:00 2001 From: migueldm Date: Fri, 28 Nov 2025 16:52:36 +0100 Subject: [PATCH 099/136] fixed subscribers cb group, noy in rt --- .../FusionLocalizer.cpp | 8 +----- .../easynav_fusion_localizer/ukf_wrapper.cpp | 25 +++++++++++-------- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 561170bb..c4aeb24b 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -83,10 +83,7 @@ void FusionLocalizer::update_rt(NavState & nav_state) if (gps_time > last_gps_stamp_[i]) { // if(true) { last_gps_stamp_[i] = gps_time; - std::cout << "FusionLocalizer: Processing GNSS sensor data " << i << std::endl; auto pose = navsatfix_to_pose(gps_data[i]->data); - std::cout << "GPS UTM POSE (X, Y): " << pose.pose.pose.position.x << ", " << - pose.pose.pose.position.y << std::endl; // nav_state.set("UTM_gnss_pose", pose); // Call the wrapper callback ukf_wrapper_->poseCallback( @@ -96,14 +93,11 @@ void FusionLocalizer::update_rt(NavState & nav_state) ukf_wrapper_->getOdomFrameId(), // pose_source_frame false // imu_data ); - std::cout << "FusionLocalizer: Processed!!" << std::endl; } } } - - std::cout << "Updating..." << std::endl; + ukf_wrapper_->periodicUpdate(); - std::cout << "Updated!!" << std::endl; nav_msgs::msg::Odometry current_odom; if (ukf_wrapper_->getFilteredOdometryMessage(¤t_odom)) { 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 index 81940439..b90c167b 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -820,6 +820,11 @@ 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); @@ -1169,7 +1174,7 @@ void UkfWrapper::loadParams() set_pose_sub_ = parent_node_->create_subscription( "set_pose", rclcpp::QoS(1), - std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1)); + std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options); // Create a service for manually setting/resetting pose set_pose_service_ = @@ -1305,7 +1310,7 @@ void UkfWrapper::loadParams() topic_subs_.push_back( parent_node_->create_subscription( odom_topic, custom_qos, - odom_callback)); + odom_callback, options)); } else { std::stringstream stream; stream << odom_topic << " is listed as an input topic, but all update " @@ -1443,7 +1448,7 @@ void UkfWrapper::loadParams() topic_subs_.push_back( parent_node_->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, custom_qos, pose_callback)); + pose_topic, custom_qos, pose_callback, options)); if (differential) { twist_var_counts[StateMemberVx] += pose_update_vec[StateMemberX]; @@ -1526,9 +1531,9 @@ void UkfWrapper::loadParams() 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); + // 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 @@ -1660,7 +1665,7 @@ void UkfWrapper::loadParams() topic_subs_.push_back( parent_node_->create_subscription< geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, custom_qos, twist_callback)); + twist_topic, custom_qos, twist_callback, options)); twist_var_counts[StateMemberVx] += twist_update_vec[StateMemberVx]; twist_var_counts[StateMemberVy] += twist_update_vec[StateMemberVy]; @@ -1869,7 +1874,7 @@ void UkfWrapper::loadParams() auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); topic_subs_.push_back( parent_node_->create_subscription( - imu_topic, custom_qos, imu_callback)); + imu_topic, custom_qos, imu_callback, options)); } else { RCLCPP_ERROR_STREAM( parent_node_->get_logger(), @@ -1950,11 +1955,11 @@ void UkfWrapper::loadParams() if(stamped_control_) { stamped_control_sub_ = parent_node_->create_subscription( "cmd_vel", rclcpp::QoS(1), - std::bind(&UkfWrapper::controlStampedCallback, this, std::placeholders::_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)); + std::bind(&UkfWrapper::controlCallback, this, std::placeholders::_1), options); } } From bd0954025345753681c23efa319ccb5d7353db3e Mon Sep 17 00:00:00 2001 From: Miguel Date: Sun, 30 Nov 2025 16:40:33 +0100 Subject: [PATCH 100/136] Update package names in rolling.yaml workflow --- .github/workflows/rolling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 788d0c04..ea793087 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -26,7 +26,7 @@ jobs: - 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 easynav_mpc_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: false From 9d681aea6e1f9b9b8691cbb97e0e95fc08872b4f Mon Sep 17 00:00:00 2001 From: migueldm Date: Mon, 1 Dec 2025 12:18:35 +0100 Subject: [PATCH 101/136] uncrustify fix --- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index c4aeb24b..60bd4e64 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -96,7 +96,7 @@ void FusionLocalizer::update_rt(NavState & nav_state) } } } - + ukf_wrapper_->periodicUpdate(); nav_msgs::msg::Odometry current_odom; From 39c780c9fac37e670d723e287fa212b0b0f2502a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 3 Dec 2025 20:37:29 +0100 Subject: [PATCH 102/136] Sync CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index fb6e79df..583ff9a6 100644 --- a/README.md +++ b/README.md @@ -69,6 +69,7 @@ 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) | --- From 922d139fbe6a2350ba95d388a5937af3304f8e97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 05:55:35 +0100 Subject: [PATCH 103/136] Publish more frequently MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_planner/CostmapPlanner.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) 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 59461fb0..08308c78 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -103,7 +103,6 @@ std::expected CostmapPlanner::on_initialize() 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; } @@ -166,7 +165,8 @@ void CostmapPlanner::update(NavState & nav_state) last_plan_time = get_node()->now(); } const double since_last = (get_node()->now() - last_plan_time).seconds(); - if (continuous_replan_ && same_start_cell && same_goal_pose && since_last < 0.05) { + // 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; @@ -175,6 +175,7 @@ void CostmapPlanner::update(NavState & nav_state) auto poses = a_star_path(map, robot_pose.pose.pose, goal); if (!poses.empty()) { + 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) { @@ -184,13 +185,9 @@ void CostmapPlanner::update(NavState & nav_state) pose_stamped.pose = pose; current_path_.poses.push_back(pose_stamped); } - // Publish only when the content changed (size as a cheap proxy) - static size_t last_published_size = 0; - if (current_path_.poses.size() != last_published_size) { - if (path_pub_->get_subscription_count() > 0) { - path_pub_->publish(current_path_); - } - last_published_size = current_path_.poses.size(); + // 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; From a8bf55f87ae5d7a7a733d106d6e507882e4c120f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 06:30:20 +0100 Subject: [PATCH 104/136] Stop controllers at IDLE MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_mpc_controller/CMakeLists.txt | 2 + .../easynav_mpc_controller/package.xml | 1 + .../easynav_mpc_controller/MPCController.cpp | 13 ++++++ .../easynav_mppi_controller/CMakeLists.txt | 2 + .../easynav_mppi_controller/package.xml | 1 + .../MPPIController.cpp | 22 +++++++++ .../CostmapPlanner.cpp | 45 +++++++++++++++++++ 7 files changed, 86 insertions(+) diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index b8e9b365..48191bd0 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -12,6 +12,7 @@ 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) @@ -44,6 +45,7 @@ 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 pluginlib::pluginlib Eigen3::Eigen diff --git a/controllers/easynav_mpc_controller/package.xml b/controllers/easynav_mpc_controller/package.xml index 52ebb254..c1ec0cb3 100644 --- a/controllers/easynav_mpc_controller/package.xml +++ b/controllers/easynav_mpc_controller/package.xml @@ -14,6 +14,7 @@ easynav_common easynav_core + easynav_system pluginlib tf2_ros geometry_msgs diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 373b1da4..c8a3e15e 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -21,6 +21,7 @@ /// \brief Implementation of the MPCController class. #include "easynav_mpc_controller/MPCController.hpp" +#include "easynav_system/GoalManager.hpp" namespace easynav { @@ -114,6 +115,18 @@ MPCController::collision_checker(void *data, std::vector & u) 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; diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index d702c4f5..275dd070 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_controller/CMakeLists.txt @@ -12,6 +12,7 @@ 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) @@ -32,6 +33,7 @@ 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 pluginlib::pluginlib ${geometry_msgs_TARGETS} diff --git a/controllers/easynav_mppi_controller/package.xml b/controllers/easynav_mppi_controller/package.xml index 7e9ee33a..f2008257 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -11,6 +11,7 @@ easynav_common easynav_core + easynav_system pluginlib tf2_ros geometry_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 783e208f..495d6c66 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -24,6 +24,7 @@ #include "easynav_mppi_controller/MPPIController.hpp" #include "easynav_common/types/PointPerception.hpp" +#include "easynav_system/GoalManager.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -140,6 +141,27 @@ 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; } 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 08308c78..ea0a3e95 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -68,6 +68,48 @@ 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( @@ -175,6 +217,9 @@ void CostmapPlanner::update(NavState & nav_state) 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; From 732bf0797a8f9066e2fb7755727cdd2219dcc809 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 06:50:27 +0100 Subject: [PATCH 105/136] Trim path for MPC MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_mpc_controller/MPCController.cpp | 42 ++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index c8a3e15e..90ea1b32 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -134,7 +134,7 @@ MPCController::update_rt(NavState & nav_state) return; } - const auto path = nav_state.get("path"); + 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; @@ -145,6 +145,46 @@ MPCController::update_rt(NavState & nav_state) 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_) { From 15ad4301dc1ac0d54e393884dfe988d8577f308c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 07:29:06 +0100 Subject: [PATCH 106/136] Final alignements MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_mpc_controller/MPCController.hpp | 4 ++ .../easynav_mpc_controller/MPCController.cpp | 62 +++++++++++++++++++ .../SerestController.cpp | 29 +++++---- 3 files changed, 84 insertions(+), 11 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 51ec31d6..e01b6e6c 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -73,6 +73,10 @@ class MPCController : public ControllerMethodBase double max_ang_vel_{1.5}; ///< Maximum angular velocity for MPC. bool verbose_{false}; ///< Value to debug on terminal + // 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. diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 90ea1b32..ceee0ca1 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -43,6 +43,9 @@ MPCController::on_initialize() 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_); @@ -50,6 +53,9 @@ MPCController::on_initialize() 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_ = @@ -267,6 +273,62 @@ MPCController::update_rt(NavState & nav_state) 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(); 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 fbe6de4a..db002c46 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -401,10 +402,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}; @@ -412,6 +409,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); @@ -482,8 +482,7 @@ SerestController::should_turn_in_place( { // 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_enter = 60.0 * std::numbers::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, @@ -506,8 +505,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_ * std::numbers::pi / 180.0; if (dist_xy_goal > stop_r) { return false; @@ -620,6 +618,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_ * (std::numbers::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); @@ -714,8 +722,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 * std::numbers::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)); } @@ -766,7 +773,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 * std::numbers::pi / 180.0); const double s_total = pd.s_acc.back(); const double dist_to_end = s_total - prj.s_star; From 6d6d06a2407c9f0d91b41764badd101591e64c1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 07:36:54 +0100 Subject: [PATCH 107/136] lintering MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_planner/CostmapPlanner.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 ea0a3e95..9c8cebff 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -94,7 +94,8 @@ static void smooth_path(std::vector & poses, int windo 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 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) { From 925c3df8d5606cc9460258baef903546a83391f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 4 Dec 2025 07:41:48 +0100 Subject: [PATCH 108/136] Translate comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../SerestController.cpp | 64 +++++++++---------- 1 file changed, 32 insertions(+), 32 deletions(-) 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 db002c46..06eee4f9 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -46,7 +46,7 @@ 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_); @@ -56,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_); @@ -64,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_); @@ -73,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_); @@ -219,29 +219,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); @@ -252,12 +252,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); @@ -265,7 +265,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); } @@ -278,12 +278,12 @@ 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)); } @@ -291,16 +291,16 @@ double SerestController::closest_obstacle_distance( const NavState & nav_state) const { - // 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"); @@ -325,7 +325,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;} @@ -338,7 +338,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(); @@ -430,7 +430,7 @@ SerestController::safety_limits( 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) { @@ -465,7 +465,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; @@ -480,16 +480,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 thr_enter = 60.0 * std::numbers::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° + // 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 * std::numbers::pi / 180.0; // enter TiP if |e_theta| > 60° + // const double thr_exit = 35.0 * PI / 180.0; // exit TiP if |e_theta| < 35° - // No permitimos “atajo” marcha atrás en esta decisión: si no permites reverse, - // el criterio es más estricto. + // 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; From 3492db69f58d7d44be17dd83c63160560622561a Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Thu, 4 Dec 2025 21:16:45 +0100 Subject: [PATCH 109/136] Collision avoidance was improved --- .../easynav_mpc_controller/MPCController.hpp | 16 ++++++---- .../easynav_mpc_controller/MPCController.cpp | 32 +++++++++++++------ 2 files changed, 33 insertions(+), 15 deletions(-) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index e01b6e6c..66edbf46 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -66,12 +66,16 @@ class MPCController : public ControllerMethodBase 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 + 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). diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index ceee0ca1..a984b68c 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -107,14 +107,26 @@ MPCController::collision_checker(void *data, std::vector & u) real_points++; } } - 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(real_points != 0 && dist < safety_radius_) { - std::cerr << "Detection at: " << dist << " Theta: " << angle << std::endl; - u[0] -= dist / real_points * dt_; - u[1] -= angle / real_points * dt_; + 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; + } } } @@ -271,7 +283,9 @@ MPCController::update_rt(NavState & nav_state) std::cerr << "Optimization Error: " << e.what() << std::endl; } - collision_checker(¶ms, u); + if (collision_checker_active_) { + collision_checker(¶ms, u); + } // Final alignment phase with hysteresis on distance: // - Enter when dist_to_goal <= 0.5 * pos_tol From a52b8d00364cea5938e3cac8a8dc79de23c860e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 5 Dec 2025 06:59:10 +0100 Subject: [PATCH 110/136] Fix link error when running MPPI in Realease MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_mppi_controller/MPPIOptimizer.cpp | 1 + 1 file changed, 1 insertion(+) 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 a7d86035..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,5 +1,6 @@ #include "easynav_mppi_controller/MPPIOptimizer.hpp" #include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include From a7a909a673873a075a6692b11ca454079ad2cc4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 5 Dec 2025 07:09:04 +0100 Subject: [PATCH 111/136] Include package in cmake and package.xml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- controllers/easynav_mppi_controller/CMakeLists.txt | 3 +++ controllers/easynav_mppi_controller/package.xml | 1 + 2 files changed, 4 insertions(+) diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index 275dd070..43a8650c 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_controller/CMakeLists.txt @@ -16,6 +16,7 @@ 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) @@ -35,6 +36,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC 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} @@ -77,6 +79,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/package.xml b/controllers/easynav_mppi_controller/package.xml index f2008257..672069e4 100644 --- a/controllers/easynav_mppi_controller/package.xml +++ b/controllers/easynav_mppi_controller/package.xml @@ -14,6 +14,7 @@ easynav_system pluginlib tf2_ros + tf2_geometry_msgs geometry_msgs nav_msgs visualization_msgs From d9b3d1bbf6d8865fd94ec1cbec55fef83fb6792b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 5 Dec 2025 19:27:20 +0100 Subject: [PATCH 112/136] Refactor to use TFInfo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_mpc_controller/MPCController.cpp | 3 ++- .../MPPIController.cpp | 9 +++++---- .../SerestController.cpp | 4 ++-- .../tests/simple_controller_tests.cpp | 6 +++++- .../easynav_vff_controller/VffController.cpp | 9 +++++---- .../AMCLLocalizer.cpp | 19 ++++++++++-------- .../easynav_gps_localizer/GpsLocalizer.cpp | 14 +++++++------ .../AMCLLocalizer.cpp | 14 +++++++------ .../AMCLLocalizer.cpp | 17 +++++++++------- .../tests/simple_localizer_tests.cpp | 8 ++++++-- .../filters/CostmapFilter.hpp | 7 ++++--- .../CostmapMapsManager.cpp | 8 ++++---- .../filters/CostmapFilter.cpp | 11 +++++----- .../filters/ObstacleFilter.cpp | 2 +- .../filters/NavMapFilter.hpp | 8 +++++--- .../NavMapMapsManager.cpp | 2 +- .../filters/NavMapFilter.cpp | 11 +++++----- .../filters/OctomapFilter.hpp | 7 ++++--- .../filters/ObstacleFilter.cpp | 2 +- .../filters/OctomapFilter.cpp | 10 +++++----- .../RoutesFilter.hpp | 5 +++-- .../filters/RoutesCostmapFilter.hpp | 9 ++++----- .../RoutesMapsManager.cpp | 2 +- .../filters/RoutesCostmapFilter.cpp | 7 +++---- .../tests/routes_costmap_filter_tests.cpp | 9 ++++++--- .../tests/routes_mapsmanager_tests.cpp | 15 +++++++++----- .../SimpleMapsManager.cpp | 8 ++++---- .../tests/simple_mapsmanager_tests.cpp | 20 +++++++++++++++---- .../CostmapPlanner.cpp | 2 +- .../easynav_navmap_planner/AStarPlanner.cpp | 2 +- .../easynav_simple_planner/SimplePlanner.cpp | 2 +- 31 files changed, 147 insertions(+), 105 deletions(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index ceee0ca1..d3f4cebe 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -201,9 +201,10 @@ MPCController::update_rt(NavState & nav_state) const auto & last_pose = path.poses[local_horizon].pose.position; const auto & perceptions = nav_state.get("points"); + const auto & tf_info = get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .filter({-2.0, -0.35, -1.0}, {0.0, 0.35, 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/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index 495d6c66..a8a5c90d 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -78,6 +78,7 @@ void MPPIController::publish_mppi_markers( const std::vector>> & all_trajs, const std::vector> & best_traj) { + const auto & tf_info = get_tf_info(); visualization_msgs::msg::MarkerArray candidates; visualization_msgs::msg::MarkerArray optimal; int id = 0; @@ -85,7 +86,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++; @@ -110,7 +111,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++; @@ -188,10 +189,10 @@ MPPIController::update_rt(NavState & nav_state) const auto & pose = nav_state.get("robot_pose").pose.pose; const auto & perceptions = nav_state.get("points"); - + const auto & tf_info = 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_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index 06eee4f9..8ed79cb6 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -306,7 +306,7 @@ SerestController::closest_obstacle_distance( const auto & perceptions = nav_state.get("points"); auto fused = PointPerceptionsOpsView(perceptions) .downsample(0.3) - .fuse(get_tf_prefix() + "base_link") + .fuse(get_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}) @@ -368,7 +368,7 @@ SerestController::fetch_required_inputs( nav_msgs::msg::Odometry & odom) { 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, get_tf_info().robot_frame); return false; } diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index af52d97e..b2af6114 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -144,7 +144,11 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) { auto node = std::make_shared("test_savemap_node"); auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + manager->initialize(node, "test_savemap", tf_info); auto static_map = std::make_shared(); static_map->initialize(4, 4, 0.5, -1.0, -1.0, 0); 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 ec916061..f12db4de 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -64,7 +64,7 @@ std::expected VffController::on_initialize() // 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 = get_tf_info().robot_frame; cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.linear.y = 0.0; cmd_vel_.twist.linear.z = 0.0; @@ -215,7 +215,7 @@ void VffController::update_rt(NavState & nav_state) const auto & all_goals = nav_state.get("goals"); if (all_goals.goals.empty()) { - cmd_vel_.header.frame_id = get_tf_prefix() + "map"; + cmd_vel_.header.frame_id = get_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; @@ -259,17 +259,18 @@ void VffController::update_rt(NavState & nav_state) const auto & perceptions = nav_state.get("points"); + const auto & tf_info = 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.robot_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(); // 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/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index cf95fa59..bd702714 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -340,7 +340,7 @@ AMCLLocalizer::init_pose_callback( auto logger = get_node()->get_logger(); // Check expected frame - const std::string expected_frame = get_tf_prefix() + std::string("map"); + const std::string expected_frame = get_tf_info().map_frame; if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { RCLCPP_WARN( logger, @@ -578,9 +578,10 @@ AMCLLocalizer::correct(NavState & nav_state) const auto & map_static = nav_state.get("map.static"); + const auto & tf_info = get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.getResolution()) - .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.getResolution()) @@ -717,8 +718,9 @@ 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"; + const auto & tf_info = 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); @@ -730,7 +732,7 @@ AMCLLocalizer::publishParticles() { 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.frame_id = get_tf_info().map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { @@ -791,7 +793,7 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) geometry_msgs::msg::PoseWithCovarianceStamped msg; msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.frame_id = get_tf_info().map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -814,8 +816,9 @@ 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"; + const auto & tf_info = 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_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index a50a7161..5c5b201c 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -34,8 +34,9 @@ std::expected GpsLocalizer::on_initialize() // 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 = 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 +58,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; @@ -114,8 +115,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 = 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/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index da00a8ce..6706af87 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -954,8 +954,9 @@ 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"; + const auto & tf_info = 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); @@ -965,7 +966,7 @@ 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"; + array_msg.header.frame_id = get_tf_info().map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { geometry_msgs::msg::Pose pose_msg; @@ -1059,7 +1060,7 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) geometry_msgs::msg::PoseWithCovarianceStamped msg; msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.frame_id = get_tf_info().map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -1082,8 +1083,9 @@ 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"; + const auto & tf_info = 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_; 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 ea441228..53ea0db2 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -386,9 +386,10 @@ void AMCLLocalizer::correct(NavState & nav_state) const auto & map_static = nav_state.get("map.static"); + const auto & tf_info = 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,8 +527,9 @@ 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"; + const auto & tf_info = 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); @@ -539,7 +541,7 @@ AMCLLocalizer::publishParticles() { 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.frame_id = get_tf_info().map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { @@ -600,7 +602,7 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) geometry_msgs::msg::PoseWithCovarianceStamped msg; msg.header.stamp = get_node()->now(); - msg.header.frame_id = get_tf_prefix() + "map"; + msg.header.frame_id = get_tf_info().map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -623,8 +625,9 @@ 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"; + const auto & tf_info = 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..6d175bf9 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -104,7 +104,11 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) { auto node = std::make_shared("test_node2"); auto manager = std::make_shared(); - manager->initialize(node, "test2"); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + manager->initialize(node, "test2", tf_info); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); @@ -114,7 +118,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_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 4f2dfef6..648cd37a 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 @@ -25,6 +25,7 @@ #include #include "easynav_common/types/NavState.hpp" +#include "easynav_common/types/TFInfo.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -46,7 +47,7 @@ class CostmapFilter initialize( const std::shared_ptr parent_node, const std::string & plugin_name, - const std::string & tf_prefix = ""); + const TFInfo & tf_info); virtual std::expected on_initialize() = 0; virtual void update(NavState & nav_state) = 0; @@ -57,12 +58,12 @@ class CostmapFilter std::shared_ptr get_node() const; - const std::string & get_tf_prefix() const; + const TFInfo & get_tf_info() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; + TFInfo tf_info_; }; } // namespace easynav 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 2c9e1cb9..6b62fb9e 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 @@ -81,7 +81,7 @@ CostmapMapsManager::on_initialize() instance = costmap_filters_loader_->createSharedInstance(plugin); auto result = instance->initialize(node, plugin_name + "." + costmap_filter, - get_tf_prefix()); + get_tf_info()); if (!result) { RCLCPP_ERROR(node->get_logger(), @@ -125,7 +125,7 @@ CostmapMapsManager::on_initialize() static_map_ = Costmap2D(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = get_tf_info().map_frame; static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); } @@ -138,7 +138,7 @@ CostmapMapsManager::on_initialize() static_map_ = Costmap2D(*msg); - static_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + static_grid_msg_.header.frame_id = get_tf_info().map_frame; static_grid_msg_.header.stamp = this->get_node()->now(); static_occ_pub_->publish(static_grid_msg_); @@ -202,7 +202,7 @@ CostmapMapsManager::update(NavState & nav_state) nav_state.set("map.dynamic", dynamic_map_); dynamic_map_->toOccupancyGridMsg(dynamic_grid_msg_); - dynamic_grid_msg_.header.frame_id = get_tf_prefix() + "map"; + dynamic_grid_msg_.header.frame_id = get_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_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 785bc235..8878a36c 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 @@ -35,12 +35,11 @@ std::expected CostmapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name, - const std::string & tf_prefix -) + const TFInfo & tf_info) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; + tf_info_ = tf_info; return on_initialize(); } @@ -57,10 +56,10 @@ CostmapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -CostmapFilter::get_tf_prefix() const +const TFInfo & +CostmapFilter::get_tf_info() const { - return tf_prefix_; + return tf_info_; } } // namespace easynav 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 0f089294..640832fe 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 @@ -59,7 +59,7 @@ ObstacleFilter::update(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map.getResolution()) - .fuse(get_tf_prefix() + "map") + .fuse(get_tf_info().map_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); 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 389992fa..294706cf 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 @@ -25,6 +25,7 @@ #include #include "easynav_common/types/NavState.hpp" +#include "easynav_common/types/TFInfo.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -41,7 +42,7 @@ class NavMapFilter initialize( const std::shared_ptr parent_node, const std::string & plugin_name, - const std::string & tf_prefix = ""); + const TFInfo & tf_info); virtual std::expected on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; @@ -54,14 +55,15 @@ class NavMapFilter const std::string & get_plugin_name() const; + const TFInfo & get_tf_info() const; + protected: std::shared_ptr get_node() const; - const std::string & get_tf_prefix() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; + TFInfo tf_info_; float map_resolution_ {0.1}; }; 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 6ba9390e..af7ee27f 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 @@ -93,7 +93,7 @@ NavMapMapsManager::on_initialize() instance = navmap_filters_loader_->createSharedInstance(plugin); auto result = instance->initialize(node, plugin_name + "." + navmap_filter, - get_tf_prefix()); + get_tf_info()); if (!result) { RCLCPP_ERROR(node->get_logger(), 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 78608d0d..0864a291 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 @@ -37,12 +37,11 @@ std::expected NavMapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name, - const std::string & tf_prefix -) + const TFInfo & tf_info) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; + tf_info_ = tf_info; return on_initialize(); } @@ -59,10 +58,10 @@ NavMapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -NavMapFilter::get_tf_prefix() const +const TFInfo & +NavMapFilter::get_tf_info() const { - return tf_prefix_; + return tf_info_; } } // namespace navmap 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 abe0948b..8178fbc2 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 @@ -26,6 +26,7 @@ #include "octomap/octomap.h" #include "easynav_common/types/NavState.hpp" +#include "easynav_common/types/TFInfo.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -42,7 +43,7 @@ class OctomapFilter initialize( const std::shared_ptr parent_node, const std::string & plugin_name, - const std::string & tf_prefix = ""); + const TFInfo & tf_info); virtual std::expected on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; @@ -55,12 +56,12 @@ class OctomapFilter const std::string & get_plugin_name() const; - const std::string & get_tf_prefix() const; + const TFInfo & get_tf_info() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - std::string tf_prefix_; + TFInfo tf_info_; float map_resolution_ {0.1}; }; 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..73e9f8c8 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 @@ -66,7 +66,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(get_map_resolution()) - .fuse(get_tf_prefix() + "map") + .fuse(get_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 2e03426a..dbb38d80 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 @@ -39,12 +39,12 @@ std::expected OctomapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name, - const std::string & tf_prefix + const TFInfo & tf_info ) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_prefix_ = tf_prefix; + tf_info_ = tf_info; return on_initialize(); } @@ -61,10 +61,10 @@ OctomapFilter::get_plugin_name() const return plugin_name_; } -const std::string & -OctomapFilter::get_tf_prefix() const +const TFInfo & +OctomapFilter::get_tf_info() const { - return tf_prefix_; + return tf_info_; } } // namespace octomap 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 index 859744fc..aacde33a 100644 --- 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 @@ -26,6 +26,7 @@ #include #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "easynav_common/types/TFInfo.hpp" namespace easynav { @@ -57,13 +58,13 @@ class RoutesFilter /// @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_prefix TF frame prefix used by the navigation stack. + /// @param tf_info TF frame information used by the navigation stack. /// @return std::expected Empty on success or /// an error message describing the failure. virtual std::expected initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns, - const std::string & tf_prefix) = 0; + const TFInfo & tf_info) = 0; /// @brief Update hook called every navigation cycle. /// 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 index 6baa4c6f..0acf50aa 100644 --- 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 @@ -53,14 +53,13 @@ class RoutesCostmapFilter : public RoutesFilter /// RoutesMapsManager. /// @param plugin_ns Namespace used to resolve this filter's /// parameters and topics. - /// @param tf_prefix TF frame prefix; used to set the frame id of - /// the published debug grid. + /// @param tf_info TF frame information used by the navigation stack. /// @return std::expected Empty on success or an /// error message on failure. std::expected initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns, - const std::string & tf_prefix) override; + const TFInfo & tf_info) override; /// @brief Apply the routes-based filtering to the costmap. /// @@ -80,8 +79,8 @@ class RoutesCostmapFilter : public RoutesFilter /// @brief Plugin namespace used for parameters and topics. std::string plugin_ns_; - /// @brief TF prefix used for published frame ids. - std::string tf_prefix_; + /// @brief TF info used in the navigation stack. + TFInfo tf_info_; /// @brief Minimum cost enforced outside the route corridor. int min_cost_{50}; 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 index f61cde44..063c2092 100644 --- 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 @@ -205,7 +205,7 @@ std::expected RoutesMapsManager::on_initialize() std::shared_ptr instance = routes_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + filter_name, get_tf_prefix()); + auto result = instance->initialize(node, plugin_name + "." + filter_name, get_tf_info()); if (!result) { RCLCPP_ERROR(node->get_logger(), "Unable to initialize RoutesFilter %s [%s]. Error: %s", 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 index 90426bfd..f26690a8 100644 --- 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 @@ -39,12 +39,11 @@ std::expected RoutesCostmapFilter::initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns, - const std::string & tf_prefix) + const TFInfo & tf_info) { node_ = node; plugin_ns_ = plugin_ns; - tf_prefix_ = tf_prefix; - + tf_info_ = tf_info; // 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_); @@ -182,7 +181,7 @@ RoutesCostmapFilter::update(NavState & nav_state) } costmap.toOccupancyGridMsg(routes_grid_msg_); - routes_grid_msg_.header.frame_id = tf_prefix_ + "map"; + routes_grid_msg_.header.frame_id = tf_info_.map_frame; if (auto node_locked = node_.lock()) { routes_grid_msg_.header.stamp = node_locked->now(); } 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 index be9bbeaf..3eccdff1 100644 --- 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 @@ -55,7 +55,8 @@ TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) auto node = std::make_shared("routes_costmap_filter_missing"); RoutesCostmapFilter filter; - auto init_result = filter.initialize(node, "routes.routes_costmap", ""); + easynav::TFInfo tf_info; + auto init_result = filter.initialize(node, "routes.routes_costmap", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); NavState nav_state; @@ -68,7 +69,8 @@ TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) auto node = std::make_shared("routes_costmap_filter_basic"); RoutesCostmapFilter filter; - auto init_result = filter.initialize(node, "routes.routes_costmap", ""); + easynav::TFInfo tf_info; + auto init_result = filter.initialize(node, "routes.routes_costmap", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); // Simple costmap 10x1, resolution 1.0, origin at (0,0) @@ -116,7 +118,8 @@ TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) auto node = std::make_shared("routes_costmap_filter_outside"); RoutesCostmapFilter filter; - auto init_result = filter.initialize(node, "routes.routes_costmap", ""); + easynav::TFInfo tf_info; + auto init_result = filter.initialize(node, "routes.routes_costmap", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); // Costmap 5x1 from x=0..5 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 index 8e834c74..26f9ae23 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -88,7 +88,8 @@ TEST_F(RoutesMapsManagerTest, LoadsRoutesFromValidYaml) }); auto manager = std::make_shared(); - auto init_result = manager->initialize(node, "routes"); + easynav::TFInfo tf_info; + auto init_result = manager->initialize(node, "routes", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -114,7 +115,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) }); auto manager = std::make_shared(); - auto init_result = manager->initialize(node, "routes"); + easynav::TFInfo tf_info; + auto init_result = manager->initialize(node, "routes", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -139,7 +141,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenYamlMissing) }); auto manager = std::make_shared(); - auto init_result = manager->initialize(node, "routes"); + easynav::TFInfo tf_info; + auto init_result = manager->initialize(node, "routes", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -165,7 +168,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenNoRoutesKey) }); auto manager = std::make_shared(); - auto init_result = manager->initialize(node, "routes"); + easynav::TFInfo tf_info; + auto init_result = manager->initialize(node, "routes", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -196,7 +200,8 @@ TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) }); auto manager = std::make_shared(); - auto init_result = manager->initialize(node, "routes"); + easynav::TFInfo tf_info; + auto init_result = manager->initialize(node, "routes", tf_info); ASSERT_TRUE(init_result.has_value()) << init_result.error(); easynav::NavState nav_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 7292bced..3dab0cb6 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 @@ -94,7 +94,7 @@ SimpleMapsManager::on_initialize() 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 = get_tf_info().map_frame; static_grid_msg_.header.stamp = this->get_node()->now(); static_occ_pub_->publish(static_grid_msg_); @@ -117,7 +117,7 @@ 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 = get_tf_info().map_frame; static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); @@ -154,7 +154,7 @@ SimpleMapsManager::update(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map_.resolution()) - .fuse(get_tf_prefix() + "map") + .fuse(get_tf_info().map_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); @@ -169,7 +169,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 = get_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 17604345..e5598a27 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 @@ -66,7 +66,11 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) { auto node = std::make_shared("test_node"); auto manager = std::make_shared(); - manager->initialize(node, "test"); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + manager->initialize(node, "test", tf_info); auto tf_buffer = easynav::RTTFBuffer::getInstance(node->get_clock()); tf2_ros::TransformListener tf_listener(*tf_buffer, *node, true); @@ -124,7 +128,11 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) { auto node = std::make_shared("test_node2"); auto manager = std::make_shared(); - manager->initialize(node, "test2"); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + manager->initialize(node, "test2", tf_info); easynav::NavState navstate; @@ -136,7 +144,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,7 +175,11 @@ TEST_F(SimpleMapsManagerTest, SavemapServiceWorks) { auto node = std::make_shared("test_savemap_node"); auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); + easynav::TFInfo tf_info; + tf_info.map_frame = "world_map"; + tf_info.odom_frame = "world_odom"; + tf_info.robot_frame = "world_base"; + manager->initialize(node, "test_savemap", tf_info); easynav::SimpleMap map_static; map_static.initialize(4, 4, 0.5, -1.0, -1.0, 0); 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 9c8cebff..47ab93d7 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -160,7 +160,7 @@ void CostmapPlanner::update(NavState & nav_state) const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; - if (goals.header.frame_id != get_tf_prefix() + "map") { + if (goals.header.frame_id != get_tf_info().map_frame) { RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s", goals.header.frame_id.c_str()); return; 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 d9dbf2fc..a0cde931 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -96,7 +96,7 @@ void AStarPlanner::update(NavState & nav_state) const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; - if (goals.header.frame_id != get_tf_prefix() + "map") { + if (goals.header.frame_id != get_tf_info().map_frame) { RCLCPP_WARN( get_node()->get_logger(), "Goals frame is not 'map': %s", goals.header.frame_id.c_str()); 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 4a902ea0..67c45535 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -135,7 +135,7 @@ SimplePlanner::update(NavState & nav_state) auto downsampled_map = map_typed.downsample(0.2); - if (goals.header.frame_id != "map") { + if (goals.header.frame_id != get_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; From 557b2557ba81de4faceeb2d6fdd1f16a20179068 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 5 Dec 2025 19:56:08 +0100 Subject: [PATCH 113/136] Refactor to use TFInfo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../FusionLocalizer.hpp | 4 --- .../easynav_fusion_localizer/ukf_wrapper.hpp | 25 ++++++++----------- .../FusionLocalizer.cpp | 17 ++++++------- .../easynav_fusion_localizer/ukf_wrapper.cpp | 19 ++++++-------- .../BonxaiMapsManager.hpp | 1 - .../BonxaiMapsManager.cpp | 7 +++--- 6 files changed, 27 insertions(+), 46 deletions(-) diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index e88eb605..c96bb975 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -55,10 +55,6 @@ class FusionLocalizer : public easynav::LocalizerMethodBase int n_imu_sensors_{0}; int n_gps_sensors_{0}; - std::string base_link_frame_id_; - std::string odom_frame_id_; - std::string world_frame_id_; - geometry_msgs::msg::PoseWithCovarianceStamped navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg); 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 index 698f85cb..9afca46b 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -50,6 +50,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "easynav_common/types/TFInfo.hpp" #include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" @@ -207,6 +208,12 @@ class UkfWrapper return filter_; } + //! @brief Returns TF configuration (map, odom, robot frames) + const easynav::TFInfo & getTFInfo() const + { + return tf_info_; + } + //! @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 @@ -348,21 +355,6 @@ class UkfWrapper return gps_callbackData_arr_; } - std::string getBaseLinkFrameId() const - { - return base_link_frame_id_; - } - - std::string getWorldFrameId() const - { - return world_frame_id_; - } - - std::string getOdomFrameId() const - { - return odom_frame_id_; - } - protected: //! @brief Finds the latest filter state before the given timestamp and makes //! it the current state again. @@ -635,6 +627,9 @@ class UkfWrapper //! std::string world_frame_id_; + //! @brief TF configuration aggregated for Easynav + easynav::TFInfo tf_info_; + //! @brief Used for outputting debug messages //! std::ofstream debug_stream_; diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 60bd4e64..11564c0e 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -28,13 +28,13 @@ std::expected FusionLocalizer::on_initialize() last_gps_stamp_.resize(10, 0.0); const std::string & plugin_name = this->get_plugin_name(); - const std::string & tf_prefix = this->get_tf_prefix(); - RCLCPP_INFO(localizer_node->get_logger(), "Using tf_prefix: '%s'", tf_prefix.c_str()); + const auto & tf_info = this->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_prefix, plugin_name + ".local_filter" + localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter" ); ukf_wrapper_->initialize(); localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); @@ -64,10 +64,6 @@ std::expected FusionLocalizer::on_initialize() n_gps_sensors_ = static_cast(ukf_wrapper_->getGpsCallbackDataArr().size()); - base_link_frame_id_ = ukf_wrapper_->getBaseLinkFrameId(); - world_frame_id_ = ukf_wrapper_->getWorldFrameId(); - odom_frame_id_ = ukf_wrapper_->getOdomFrameId(); - RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); return {}; } @@ -86,11 +82,12 @@ void FusionLocalizer::update_rt(NavState & nav_state) auto pose = navsatfix_to_pose(gps_data[i]->data); // nav_state.set("UTM_gnss_pose", pose); // Call the wrapper callback + const auto & tf_info = get_tf_info(); ukf_wrapper_->poseCallback( std::make_shared(pose), ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data - ukf_wrapper_->getWorldFrameId(), // target_frame - ukf_wrapper_->getOdomFrameId(), // pose_source_frame + tf_info.world_frame, // target_frame + tf_info.odom_frame, // pose_source_frame false // imu_data ); } @@ -120,7 +117,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose // 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 = world_frame_id_; + pose_msg.header.frame_id = get_tf_info().world_frame; // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) double utm_x, utm_y; 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 index b90c167b..35a91691 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -885,17 +885,11 @@ void UkfWrapper::loadParams() } } - // These params specify the name of the robot's body frame (typically - // base_link) and odometry frame (typically odom) - map_frame_id_ = parent_node_->declare_parameter(param_prefix + "map_frame", std::string("map")); - odom_frame_id_ = parent_node_->declare_parameter(param_prefix + "odom_frame", - std::string("odom")); - base_link_frame_id_ = parent_node_->declare_parameter(param_prefix + - "base_link_frame", - std::string("base_link")); - base_link_output_frame_id_ = parent_node_->declare_parameter(param_prefix + - "base_link_frame_output", - base_link_frame_id_); + // Frame configuration comes from Easynav TFInfo; no frame params declared here + map_frame_id_ = tf_info_.map_frame; + odom_frame_id_ = tf_info_.odom_frame; + base_link_frame_id_ = tf_info_.robot_frame; + base_link_output_frame_id_ = base_link_frame_id_; /* * These parameters are designed to enforce compliance with REP-105: @@ -922,7 +916,8 @@ void UkfWrapper::loadParams() * * The default is the latter behavior (broadcast of odom->base_link). */ - world_frame_id_ = parent_node_->declare_parameter(param_prefix + "world_frame", odom_frame_id_); + // World frame comes from Easynav TFInfo configuration + world_frame_id_ = tf_info_.world_frame; if (map_frame_id_ == odom_frame_id_ || odom_frame_id_ == base_link_frame_id_ || 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 fd2ba8cd..d9b2694f 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 @@ -118,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/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp index ba54589c..53c23a73 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 @@ -64,13 +64,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", @@ -181,7 +179,8 @@ BonxaiMapsManager::update_from_pc2(const sensor_msgs::msg::PointCloud2 & pc2) 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)); + get_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; @@ -285,7 +284,7 @@ BonxaiMapsManager::update_from_occ(const nav_msgs::msg::OccupancyGrid & occ) void BonxaiMapsManager::publish_map() { - bonxai_msg_.header.frame_id = frame_id_; + bonxai_msg_.header.frame_id = get_tf_info().map_frame; bonxai_msg_.header.stamp = this->get_node()->now(); bonxai_pub_->publish(bonxai_msg_); From 5812bb642066217222eb4ff5b3b636ad47708d6f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 6 Dec 2025 08:32:33 +0100 Subject: [PATCH 114/136] Fix downsample in Simple MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_simple_common/SimpleMap.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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); } } From bc9a68a5f24432574999e6ebb84869d664737230 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 6 Dec 2025 09:38:09 +0100 Subject: [PATCH 115/136] Improve heuristic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- planners/easynav_costmap_planner/README.md | 48 +++++++++++++++++-- .../CostmapPlanner.hpp | 3 +- .../CostmapPlanner.cpp | 19 ++++---- 3 files changed, 56 insertions(+), 14 deletions(-) diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index dbea0d9e..d21e367c 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -31,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 9df455ef..c852a09c 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -70,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 47ab93d7..d58ad0e1 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -128,15 +128,12 @@ std::expected CostmapPlanner::on_initialize() 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( @@ -259,8 +256,8 @@ std::vector CostmapPlanner::a_star_path( 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 = cost_axial_; - const double diagonal_cost = cost_diagonal_; + 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; @@ -271,7 +268,9 @@ std::vector CostmapPlanner::a_star_path( std::vector parent_y(total_cells, -1); std::vector cost_so_far(total_cells, std::numeric_limits::infinity()); - open.push(GridNode{static_cast(sx), static_cast(sy), 0.0, heuristic(sx, sy, gx, gy)}); + 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()) { @@ -297,7 +296,9 @@ std::vector CostmapPlanner::a_star_path( 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)}); + 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; } From fca37df3a45581e0f551d8af10bf5971b3195170 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 6 Dec 2025 10:48:12 +0100 Subject: [PATCH 116/136] Final adjustements MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_localizer/AMCLLocalizer.cpp | 3 ++- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 11 +++++++---- .../NavMapMapsManager.cpp | 8 ++++---- .../filters/ObstacleFilter.cpp | 2 +- .../OctomapMapsManager.cpp | 5 +++-- .../RoutesMapsManager.cpp | 14 +++++++------- 6 files changed, 24 insertions(+), 19 deletions(-) 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 bd702714..d9ba5d11 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -488,7 +488,8 @@ AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); + get_tf_info().odom_frame, get_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; 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 6706af87..2c4f1b24 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -484,7 +484,8 @@ void AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - "odom", "base_footprint", tf2::TimePointZero, tf2::durationFromSec(0.0)); + get_tf_info().odom_frame, get_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; @@ -620,12 +621,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; } @@ -744,7 +747,7 @@ void AMCLLocalizer::correct(NavState & nav_state) for (const auto & b : bundles) { if (!T_bf_sensor_cache.count(b.frame_id)) { 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(get_tf_info().robot_frame, b.frame_id); // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; 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 af7ee27f..69b5d2c7 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 @@ -140,7 +140,7 @@ NavMapMapsManager::on_initialize() 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 = get_tf_info().map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } @@ -155,7 +155,7 @@ NavMapMapsManager::on_initialize() 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 = get_tf_info().map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } else { @@ -172,7 +172,7 @@ NavMapMapsManager::on_initialize() 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 = get_tf_info().map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); @@ -186,7 +186,7 @@ NavMapMapsManager::on_initialize() navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); - navmap_msg_.header.frame_id = "map"; + navmap_msg_.header.frame_id = get_tf_info().map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); 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 9eaba396..c1d0edbd 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 @@ -60,7 +60,7 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const auto & points = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) .downsample(0.3) - .fuse("map") + .fuse(get_tf_info().map_frame) .as_points(); const float voxel_xy = 0.30f; 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 cc3a05b0..b4d59ed3 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 @@ -179,7 +179,8 @@ OctomapMapsManager::on_initialize() 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)); + get_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; @@ -223,7 +224,7 @@ OctomapMapsManager::on_initialize() octomap_->updateInnerOccupancy(); - octomap_msg_.header.frame_id = "map"; + octomap_msg_.header.frame_id = get_tf_info().map_frame; octomap_msg_.header.stamp = this->get_node()->now(); octomap_msg_.id = "OcTree"; octomap_msg_.binary = true; 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 index 063c2092..dee5d7d6 100644 --- 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 @@ -392,7 +392,7 @@ void RoutesMapsManager::publish_routes_markers() // removed segments do not leave orphaned markers behind. { visualization_msgs::msg::Marker m; - m.header.frame_id = "map"; + m.header.frame_id = get_tf_info().map_frame; m.action = visualization_msgs::msg::Marker::DELETEALL; m.ns = "routes_line"; @@ -406,7 +406,7 @@ void RoutesMapsManager::publish_routes_markers() for (const auto & seg : routes_) { // Line between start and end visualization_msgs::msg::Marker line; - line.header.frame_id = "map"; + line.header.frame_id = get_tf_info().map_frame; line.ns = "routes_line"; line.id = id++; line.type = visualization_msgs::msg::Marker::LINE_LIST; @@ -432,7 +432,7 @@ void RoutesMapsManager::publish_routes_markers() // Arrow for start orientation (same style as end) visualization_msgs::msg::Marker start_arrow; - start_arrow.header.frame_id = "map"; + start_arrow.header.frame_id = get_tf_info().map_frame; start_arrow.ns = "routes_arrow"; start_arrow.id = id++; start_arrow.type = visualization_msgs::msg::Marker::ARROW; @@ -449,7 +449,7 @@ void RoutesMapsManager::publish_routes_markers() // Arrow for end orientation (same style) visualization_msgs::msg::Marker end_arrow; - end_arrow.header.frame_id = "map"; + end_arrow.header.frame_id = get_tf_info().map_frame; end_arrow.ns = "routes_arrow"; end_arrow.id = id++; end_arrow.type = visualization_msgs::msg::Marker::ARROW; @@ -478,7 +478,7 @@ void RoutesMapsManager::publish_interactive_markers() 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 = "map"; + mode_marker.header.frame_id = get_tf_info().map_frame; mode_marker.name = seg.id + "_mode"; mode_marker.scale = 1.0; @@ -540,14 +540,14 @@ void RoutesMapsManager::publish_interactive_markers() } visualization_msgs::msg::InteractiveMarker start_marker; - start_marker.header.frame_id = "map"; + start_marker.header.frame_id = get_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 = "map"; + end_marker.header.frame_id = get_tf_info().map_frame; end_marker.name = seg.id + "_end"; end_marker.description = "Route " + seg.id + " end"; end_marker.pose = seg.end; From 1f6998b52f33a697bbf9a2105c3315b24f34742f Mon Sep 17 00:00:00 2001 From: migueldm Date: Wed, 10 Dec 2025 10:54:09 +0100 Subject: [PATCH 117/136] Fixed frames. In robot_localization, world frame must be odom or map depending on filter mode: local or global. This first version works as only global filter, so I changed world frame to map --- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 4 ++-- .../src/easynav_fusion_localizer/ukf_wrapper.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 11564c0e..58b34e44 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -86,7 +86,7 @@ void FusionLocalizer::update_rt(NavState & nav_state) ukf_wrapper_->poseCallback( std::make_shared(pose), ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data - tf_info.world_frame, // target_frame + tf_info.map_frame, // target_frame tf_info.odom_frame, // pose_source_frame false // imu_data ); @@ -117,7 +117,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose // 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 = get_tf_info().world_frame; + pose_msg.header.frame_id = get_tf_info().map_frame; // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) double utm_x, utm_y; 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 index 35a91691..ad14f5e8 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -908,7 +908,7 @@ void UkfWrapper::loadParams() * 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 odom_frame + * "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 @@ -917,7 +917,7 @@ void UkfWrapper::loadParams() * The default is the latter behavior (broadcast of odom->base_link). */ // World frame comes from Easynav TFInfo configuration - world_frame_id_ = tf_info_.world_frame; + world_frame_id_ = tf_info_.map_frame; if (map_frame_id_ == odom_frame_id_ || odom_frame_id_ == base_link_frame_id_ || From b4799084aaeadac06a2082c75cccbaa657fcfc32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 10 Dec 2025 20:52:34 +0100 Subject: [PATCH 118/136] TFInfo in RTTFBuffer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../tests/simple_maps_tests.cpp | 8 +++---- .../easynav_mpc_controller/MPCController.cpp | 5 ++++- .../MPPIController.cpp | 6 +++-- .../SerestController.cpp | 9 ++++++-- .../tests/simple_controller_tests.cpp | 4 +++- .../easynav_vff_controller/VffController.cpp | 9 +++++--- .../AMCLLocalizer.cpp | 22 +++++++++++-------- .../easynav_fusion_localizer/ukf_wrapper.hpp | 11 +--------- .../FusionLocalizer.cpp | 8 ++++--- .../easynav_fusion_localizer/ukf_wrapper.cpp | 15 ++++++++----- .../easynav_gps_localizer/GpsLocalizer.cpp | 4 ++-- .../AMCLLocalizer.cpp | 19 +++++++++++----- .../AMCLLocalizer.cpp | 14 +++++++----- .../tests/simple_localizer_tests.cpp | 4 +++- .../BonxaiMapsManager.cpp | 7 ++++-- .../filters/CostmapFilter.hpp | 8 +------ .../CostmapMapsManager.cpp | 18 +++++++++------ .../filters/CostmapFilter.cpp | 9 +------- .../filters/ObstacleFilter.cpp | 3 ++- .../filters/NavMapFilter.hpp | 7 +----- .../NavMapMapsManager.cpp | 19 +++++++++------- .../filters/NavMapFilter.cpp | 10 +-------- .../filters/ObstacleFilter.cpp | 4 +++- .../filters/OctomapFilter.hpp | 7 +----- .../OctomapMapsManager.cpp | 9 ++++---- .../filters/ObstacleFilter.cpp | 3 ++- .../filters/OctomapFilter.cpp | 12 +--------- .../RoutesFilter.hpp | 4 +--- .../filters/RoutesCostmapFilter.hpp | 6 +---- .../RoutesMapsManager.cpp | 21 +++++++++++------- .../filters/RoutesCostmapFilter.cpp | 8 +++---- .../tests/routes_costmap_filter_tests.cpp | 13 ++++++++--- .../tests/routes_mapsmanager_tests.cpp | 18 ++++++++++----- .../SimpleMapsManager.cpp | 15 ++++++++----- .../tests/simple_mapsmanager_tests.cpp | 12 +++++++--- .../CostmapPlanner.cpp | 4 +++- .../easynav_navmap_planner/AStarPlanner.cpp | 4 +++- .../easynav_simple_planner/SimplePlanner.cpp | 4 +++- 38 files changed, 198 insertions(+), 165 deletions(-) diff --git a/common/easynav_simple_common/tests/simple_maps_tests.cpp b/common/easynav_simple_common/tests/simple_maps_tests.cpp index 62a833b8..7ad1b184 100644 --- a/common/easynav_simple_common/tests/simple_maps_tests.cpp +++ b/common/easynav_simple_common/tests/simple_maps_tests.cpp @@ -172,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 @@ -195,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/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index d3f4cebe..fde49925 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -23,6 +23,8 @@ #include "easynav_mpc_controller/MPCController.hpp" #include "easynav_system/GoalManager.hpp" +#include "easynav_common/RTTFBuffer.hpp" + namespace easynav { @@ -201,7 +203,8 @@ MPCController::update_rt(NavState & nav_state) const auto & last_pose = path.poses[local_horizon].pose.position; const auto & perceptions = nav_state.get("points"); - const auto & tf_info = get_tf_info(); + 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) 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 a8a5c90d..14d32002 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -24,6 +24,8 @@ #include "easynav_mppi_controller/MPPIController.hpp" #include "easynav_common/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" + #include "easynav_system/GoalManager.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -78,7 +80,7 @@ void MPPIController::publish_mppi_markers( const std::vector>> & all_trajs, const std::vector> & best_traj) { - const auto & tf_info = get_tf_info(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); visualization_msgs::msg::MarkerArray candidates; visualization_msgs::msg::MarkerArray optimal; int id = 0; @@ -189,7 +191,7 @@ MPPIController::update_rt(NavState & nav_state) const auto & pose = nav_state.get("robot_pose").pose.pose; const auto & perceptions = nav_state.get("points"); - const auto & tf_info = get_tf_info(); + 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(tf_info.map_frame) 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 8ed79cb6..25c282e6 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -29,6 +29,7 @@ #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" @@ -304,9 +305,11 @@ SerestController::closest_obstacle_distance( if (!nav_state.has("points")) {return std::numeric_limits::infinity();} const auto & perceptions = nav_state.get("points"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + auto fused = PointPerceptionsOpsView(perceptions) .downsample(0.3) - .fuse(get_tf_info().robot_frame) + .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}) @@ -367,8 +370,10 @@ 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, get_tf_info().robot_frame); + publish_stop(nav_state, tf_info.robot_frame); return false; } diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index b2af6114..e783cd6c 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -148,7 +148,9 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - manager->initialize(node, "test_savemap", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + manager->initialize(node, "test_savemap"); auto static_map = std::make_shared(); static_map->initialize(4, 4, 0.5, -1.0, -1.0, 0); 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 f12db4de..9788ac4e 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -62,9 +62,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_info().robot_frame; + cmd_vel_.header.frame_id = tf_info.robot_frame; cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.linear.y = 0.0; cmd_vel_.twist.linear.z = 0.0; @@ -213,9 +215,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_info().map_frame; + 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; @@ -259,7 +262,7 @@ void VffController::update_rt(NavState & nav_state) const auto & perceptions = nav_state.get("points"); - const auto & tf_info = get_tf_info(); + 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}) 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 d9ba5d11..882c40b7 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -338,9 +338,9 @@ AMCLLocalizer::init_pose_callback( } auto logger = get_node()->get_logger(); - + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); // Check expected frame - const std::string expected_frame = get_tf_info().map_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, @@ -485,10 +485,12 @@ AMCLLocalizer::init_pose_callback( 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( - get_tf_info().odom_frame, get_tf_info().robot_frame, tf2::TimePointZero, + 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()); @@ -579,7 +581,7 @@ AMCLLocalizer::correct(NavState & nav_state) const auto & map_static = nav_state.get("map.static"); - const auto & tf_info = get_tf_info(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.getResolution()) .fuse(tf_info.robot_frame) @@ -719,7 +721,7 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; tf_msg.header.stamp = get_node()->now(); - const auto & tf_info = get_tf_info(); + 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); @@ -731,10 +733,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_info().map_frame; - + 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; @@ -792,9 +795,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_info().map_frame; + msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -817,7 +821,7 @@ AMCLLocalizer::get_pose() nav_msgs::msg::Odometry odom_msg; odom_msg.header.stamp = get_node()->now(); - const auto & tf_info = get_tf_info(); + 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; 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 index 9afca46b..d9dbde9c 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -50,7 +50,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "easynav_common/types/TFInfo.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" @@ -208,12 +208,6 @@ class UkfWrapper return filter_; } - //! @brief Returns TF configuration (map, odom, robot frames) - const easynav::TFInfo & getTFInfo() const - { - return tf_info_; - } - //! @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 @@ -627,9 +621,6 @@ class UkfWrapper //! std::string world_frame_id_; - //! @brief TF configuration aggregated for Easynav - easynav::TFInfo tf_info_; - //! @brief Used for outputting debug messages //! std::ofstream debug_stream_; diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 11564c0e..851c6c22 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -28,7 +28,8 @@ std::expected FusionLocalizer::on_initialize() last_gps_stamp_.resize(10, 0.0); const std::string & plugin_name = this->get_plugin_name(); - const auto & tf_info = this->get_tf_info(); + 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()); @@ -82,7 +83,7 @@ void FusionLocalizer::update_rt(NavState & nav_state) auto pose = navsatfix_to_pose(gps_data[i]->data); // nav_state.set("UTM_gnss_pose", pose); // Call the wrapper callback - const auto & tf_info = get_tf_info(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); ukf_wrapper_->poseCallback( std::make_shared(pose), ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data @@ -112,12 +113,13 @@ 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 = get_tf_info().world_frame; + pose_msg.header.frame_id = tf_info.world_frame; // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) double utm_x, utm_y; 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 index 35a91691..1031cdab 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -77,6 +77,8 @@ #include #include +#include "easynav_common/RTTFBuffer.hpp" + namespace robot_localization { using namespace std::chrono_literals; @@ -886,9 +888,14 @@ void UkfWrapper::loadParams() } // Frame configuration comes from Easynav TFInfo; no frame params declared here - map_frame_id_ = tf_info_.map_frame; - odom_frame_id_ = tf_info_.odom_frame; - base_link_frame_id_ = tf_info_.robot_frame; + + 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_id_ = tf_info.world_frame; + base_link_output_frame_id_ = base_link_frame_id_; /* @@ -916,8 +923,6 @@ void UkfWrapper::loadParams() * * The default is the latter behavior (broadcast of odom->base_link). */ - // World frame comes from Easynav TFInfo configuration - world_frame_id_ = tf_info_.world_frame; if (map_frame_id_ == odom_frame_id_ || odom_frame_id_ == base_link_frame_id_ || 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 5c5b201c..5ed4393f 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -34,7 +34,7 @@ std::expected GpsLocalizer::on_initialize() // Initialize the odometry message odom_.header.stamp = get_node()->now(); - const auto & tf_info = get_tf_info(); + 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; @@ -115,7 +115,7 @@ void GpsLocalizer::update(NavState & nav_state) // Get XY cartesian coordinates respect to the origin odom_.header.stamp = gps_msg_.header.stamp; - const auto & tf_info = get_tf_info(); + 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; 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 2c4f1b24..4fcebf88 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -481,10 +481,12 @@ void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) 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( - get_tf_info().odom_frame, get_tf_info().robot_frame, tf2::TimePointZero, + 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()); @@ -746,8 +748,9 @@ 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(get_tf_info().robot_frame, 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]; @@ -957,7 +960,7 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) } geometry_msgs::msg::TransformStamped tf_msg; tf_msg.header.stamp = get_node()->now(); - const auto & tf_info = get_tf_info(); + 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); @@ -967,9 +970,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_info().map_frame; + 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; @@ -1061,9 +1066,11 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) 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_info().map_frame; + msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -1086,7 +1093,7 @@ AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; odom_msg.header.stamp = get_node()->now(); - const auto & tf_info = get_tf_info(); + 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; 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 53ea0db2..7cef1851 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -386,7 +386,7 @@ void AMCLLocalizer::correct(NavState & nav_state) const auto & map_static = nav_state.get("map.static"); - const auto & tf_info = get_tf_info(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.resolution()) .fuse(tf_info.robot_frame) @@ -527,7 +527,7 @@ AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; tf_msg.header.stamp = get_node()->now(); - const auto & tf_info = get_tf_info(); + 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); @@ -539,9 +539,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_info().map_frame; + array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { @@ -600,9 +602,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_info().map_frame; + msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); msg.pose.pose.position.y = mean.y(); @@ -625,7 +629,7 @@ AMCLLocalizer::get_pose() nav_msgs::msg::Odometry odom_msg; odom_msg.header.stamp = get_node()->now(); - const auto & tf_info = get_tf_info(); + 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; diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index 6d175bf9..0d48e663 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -108,7 +108,9 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - manager->initialize(node, "test2", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + manager->initialize(node, "test2"); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); 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 53c23a73..d2399860 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 @@ -175,11 +175,12 @@ 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( - get_tf_info().map_frame, pc2.header.frame_id, pc2.header.stamp, + 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()); @@ -284,7 +285,9 @@ BonxaiMapsManager::update_from_occ(const nav_msgs::msg::OccupancyGrid & occ) void BonxaiMapsManager::publish_map() { - bonxai_msg_.header.frame_id = get_tf_info().map_frame; + 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_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 648cd37a..f830830b 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 @@ -25,7 +25,6 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/TFInfo.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -46,8 +45,7 @@ class CostmapFilter std::expected initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const TFInfo & tf_info); + const std::string & plugin_name); virtual std::expected on_initialize() = 0; virtual void update(NavState & nav_state) = 0; @@ -57,13 +55,9 @@ class CostmapFilter protected: std::shared_ptr get_node() const; - - const TFInfo & get_tf_info() const; - protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - TFInfo tf_info_; }; } // namespace easynav 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 6b62fb9e..9b2b0379 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 @@ -26,6 +26,7 @@ #include "easynav_costmap_common/costmap_2d.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" @@ -80,8 +81,7 @@ CostmapMapsManager::on_initialize() std::shared_ptr instance; instance = costmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + costmap_filter, - get_tf_info()); + auto result = instance->initialize(node, plugin_name + "." + costmap_filter); if (!result) { RCLCPP_ERROR(node->get_logger(), @@ -109,6 +109,8 @@ 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 { @@ -125,7 +127,7 @@ CostmapMapsManager::on_initialize() static_map_ = Costmap2D(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_info().map_frame; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); } @@ -133,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_info().map_frame; + 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_); @@ -146,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) { @@ -201,8 +203,10 @@ CostmapMapsManager::update(NavState & nav_state) nav_state.set("map.dynamic", dynamic_map_); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + dynamic_map_->toOccupancyGridMsg(dynamic_grid_msg_); - dynamic_grid_msg_.header.frame_id = get_tf_info().map_frame; + 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_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 8878a36c..7d2b8f2a 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 @@ -34,12 +34,10 @@ CostmapFilter::CostmapFilter() std::expected CostmapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const TFInfo & tf_info) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_info_ = tf_info; return on_initialize(); } @@ -56,10 +54,5 @@ CostmapFilter::get_plugin_name() const return plugin_name_; } -const TFInfo & -CostmapFilter::get_tf_info() const -{ - return tf_info_; -} } // namespace easynav 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 640832fe..b8a69cb5 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 @@ -56,10 +56,11 @@ ObstacleFilter::update(NavState & nav_state) 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(); auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map.getResolution()) - .fuse(get_tf_info().map_frame) + .fuse(tf_info.map_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); 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 294706cf..9bda7243 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 @@ -25,7 +25,6 @@ #include #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/TFInfo.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -41,8 +40,7 @@ class NavMapFilter std::expected initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const TFInfo & tf_info); + const std::string & plugin_name); virtual std::expected on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; @@ -55,15 +53,12 @@ class NavMapFilter const std::string & get_plugin_name() const; - const TFInfo & get_tf_info() const; - protected: std::shared_ptr get_node() const; protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - TFInfo tf_info_; float map_resolution_ {0.1}; }; 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 69b5d2c7..c1cb22f8 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 @@ -23,6 +23,7 @@ #include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" #include "easynav_common/YTSession.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -92,8 +93,7 @@ NavMapMapsManager::on_initialize() std::shared_ptr instance; instance = navmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + navmap_filter, - get_tf_info()); + auto result = instance->initialize(node, plugin_name + "." + navmap_filter); if (!result) { RCLCPP_ERROR(node->get_logger(), @@ -122,6 +122,9 @@ 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); @@ -140,7 +143,7 @@ NavMapMapsManager::on_initialize() navmap_ = navmap_ros::from_occupancy_grid(occ_msg); navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = get_tf_info().map_frame; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } @@ -155,7 +158,7 @@ NavMapMapsManager::on_initialize() if (navmap_ros::io::load_from_file(map_path_, navmap_)) { navmap_msg_ = navmap_ros::to_msg(navmap_); - navmap_msg_.header.frame_id = get_tf_info().map_frame; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = node->now(); navmap_pub_->publish(navmap_msg_); } else { @@ -166,13 +169,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 = get_tf_info().map_frame; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); @@ -180,13 +183,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 = get_tf_info().map_frame; + navmap_msg_.header.frame_id = tf_info.map_frame; navmap_msg_.header.stamp = this->get_node()->now(); navmap_pub_->publish(navmap_msg_); }); 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 0864a291..970c8e17 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 @@ -36,12 +36,10 @@ NavMapFilter::NavMapFilter() std::expected NavMapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const TFInfo & tf_info) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_info_ = tf_info; return on_initialize(); } @@ -58,11 +56,5 @@ NavMapFilter::get_plugin_name() const return plugin_name_; } -const TFInfo & -NavMapFilter::get_tf_info() const -{ - return tf_info_; -} - } // 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 c1d0edbd..c70dc614 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 @@ -24,6 +24,7 @@ #include "easynav_common/types/NavState.hpp" #include "easynav_common/types/PointPerception.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -54,13 +55,14 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) const auto & perceptions = nav_state.get("points"); navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); navmap_.layer_clear(get_layer_name(), navmap_ros::FREE_SPACE); const auto & points = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, NAN}, {10.0, 10.0, NAN}) .downsample(0.3) - .fuse(get_tf_info().map_frame) + .fuse(tf_info.map_frame) .as_points(); const float voxel_xy = 0.30f; 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 8178fbc2..6561d7ec 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 @@ -26,7 +26,6 @@ #include "octomap/octomap.h" #include "easynav_common/types/NavState.hpp" -#include "easynav_common/types/TFInfo.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -42,8 +41,7 @@ class OctomapFilter std::expected initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const TFInfo & tf_info); + const std::string & plugin_name); virtual std::expected on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; @@ -56,12 +54,9 @@ class OctomapFilter const std::string & get_plugin_name() const; - const TFInfo & get_tf_info() const; - protected: std::shared_ptr parent_node_ {nullptr}; std::string plugin_name_; - TFInfo tf_info_; float map_resolution_ {0.1}; }; 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 b4d59ed3..347bd7e6 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 @@ -171,15 +171,17 @@ 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( - get_tf_info().map_frame, msg->header.frame_id, msg->header.stamp, + 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()); @@ -223,8 +225,7 @@ OctomapMapsManager::on_initialize() octomap_->insertPointCloud(cloud, origin, 1000.0, true, false); octomap_->updateInnerOccupancy(); - - octomap_msg_.header.frame_id = get_tf_info().map_frame; + 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; 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 73e9f8c8..df331549 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 @@ -58,6 +58,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 +67,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(get_map_resolution()) - .fuse(get_tf_info().map_frame) + .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 dbb38d80..ebc222e2 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 @@ -38,14 +38,10 @@ OctomapFilter::OctomapFilter() std::expected OctomapFilter::initialize( const std::shared_ptr parent_node, - const std::string & plugin_name, - const TFInfo & tf_info -) + const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - tf_info_ = tf_info; - return on_initialize(); } @@ -61,11 +57,5 @@ OctomapFilter::get_plugin_name() const return plugin_name_; } -const TFInfo & -OctomapFilter::get_tf_info() const -{ - return tf_info_; -} - } // namespace octomap } // namespace easynav 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 index aacde33a..f3b1e1ba 100644 --- 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 @@ -26,7 +26,6 @@ #include #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "easynav_common/types/TFInfo.hpp" namespace easynav { @@ -63,8 +62,7 @@ class RoutesFilter /// an error message describing the failure. virtual std::expected initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, - const std::string & plugin_ns, - const TFInfo & tf_info) = 0; + const std::string & plugin_ns) = 0; /// @brief Update hook called every navigation cycle. /// 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 index 0acf50aa..449bb743 100644 --- 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 @@ -58,8 +58,7 @@ class RoutesCostmapFilter : public RoutesFilter /// error message on failure. std::expected initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, - const std::string & plugin_ns, - const TFInfo & tf_info) override; + const std::string & plugin_ns) override; /// @brief Apply the routes-based filtering to the costmap. /// @@ -79,9 +78,6 @@ class RoutesCostmapFilter : public RoutesFilter /// @brief Plugin namespace used for parameters and topics. std::string plugin_ns_; - /// @brief TF info used in the navigation stack. - TFInfo tf_info_; - /// @brief Minimum cost enforced outside the route corridor. int min_cost_{50}; 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 index dee5d7d6..12f27b29 100644 --- 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 @@ -19,6 +19,7 @@ #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include #include @@ -205,7 +206,7 @@ std::expected RoutesMapsManager::on_initialize() std::shared_ptr instance = routes_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + filter_name, get_tf_info()); + auto result = instance->initialize(node, plugin_name + "." + filter_name); if (!result) { RCLCPP_ERROR(node->get_logger(), "Unable to initialize RoutesFilter %s [%s]. Error: %s", @@ -386,13 +387,15 @@ void RoutesMapsManager::publish_routes_markers() 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 = get_tf_info().map_frame; + m.header.frame_id = tf_info.map_frame; m.action = visualization_msgs::msg::Marker::DELETEALL; m.ns = "routes_line"; @@ -406,7 +409,7 @@ void RoutesMapsManager::publish_routes_markers() for (const auto & seg : routes_) { // Line between start and end visualization_msgs::msg::Marker line; - line.header.frame_id = get_tf_info().map_frame; + line.header.frame_id = tf_info.map_frame; line.ns = "routes_line"; line.id = id++; line.type = visualization_msgs::msg::Marker::LINE_LIST; @@ -432,7 +435,7 @@ void RoutesMapsManager::publish_routes_markers() // Arrow for start orientation (same style as end) visualization_msgs::msg::Marker start_arrow; - start_arrow.header.frame_id = get_tf_info().map_frame; + 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; @@ -449,7 +452,7 @@ void RoutesMapsManager::publish_routes_markers() // Arrow for end orientation (same style) visualization_msgs::msg::Marker end_arrow; - end_arrow.header.frame_id = get_tf_info().map_frame; + 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; @@ -475,10 +478,12 @@ void RoutesMapsManager::publish_interactive_markers() } 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 = get_tf_info().map_frame; + mode_marker.header.frame_id = tf_info.map_frame; mode_marker.name = seg.id + "_mode"; mode_marker.scale = 1.0; @@ -540,14 +545,14 @@ void RoutesMapsManager::publish_interactive_markers() } visualization_msgs::msg::InteractiveMarker start_marker; - start_marker.header.frame_id = get_tf_info().map_frame; + 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 = get_tf_info().map_frame; + 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; 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 index f26690a8..adf222e6 100644 --- 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 @@ -30,6 +30,8 @@ #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" +#include "easynav_common/RTTFBuffer.hpp" + namespace easynav { @@ -38,12 +40,10 @@ RoutesCostmapFilter::RoutesCostmapFilter() = default; std::expected RoutesCostmapFilter::initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, - const std::string & plugin_ns, - const TFInfo & tf_info) + const std::string & plugin_ns) { node_ = node; plugin_ns_ = plugin_ns; - tf_info_ = tf_info; // 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_); @@ -181,7 +181,7 @@ RoutesCostmapFilter::update(NavState & nav_state) } costmap.toOccupancyGridMsg(routes_grid_msg_); - routes_grid_msg_.header.frame_id = tf_info_.map_frame; + 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(); } 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 index 3eccdff1..a0e2d988 100644 --- 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 @@ -20,6 +20,7 @@ #include #include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "easynav_costmap_common/costmap_2d.hpp" @@ -56,7 +57,9 @@ TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) RoutesCostmapFilter filter; easynav::TFInfo tf_info; - auto init_result = filter.initialize(node, "routes.routes_costmap", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto init_result = filter.initialize(node, "routes.routes_costmap"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); NavState nav_state; @@ -70,7 +73,9 @@ TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) RoutesCostmapFilter filter; easynav::TFInfo tf_info; - auto init_result = filter.initialize(node, "routes.routes_costmap", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto init_result = filter.initialize(node, "routes.routes_costmap"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); // Simple costmap 10x1, resolution 1.0, origin at (0,0) @@ -119,7 +124,9 @@ TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) RoutesCostmapFilter filter; easynav::TFInfo tf_info; - auto init_result = filter.initialize(node, "routes.routes_costmap", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto init_result = filter.initialize(node, "routes.routes_costmap"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); // Costmap 5x1 from x=0..5 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 index 26f9ae23..6f845450 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -22,6 +22,7 @@ #include #include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" @@ -89,7 +90,9 @@ TEST_F(RoutesMapsManagerTest, LoadsRoutesFromValidYaml) auto manager = std::make_shared(); easynav::TFInfo tf_info; - auto init_result = manager->initialize(node, "routes", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto init_result = manager->initialize(node, "routes"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -116,7 +119,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) auto manager = std::make_shared(); easynav::TFInfo tf_info; - auto init_result = manager->initialize(node, "routes", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + auto init_result = manager->initialize(node, "routes"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -142,7 +146,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenYamlMissing) auto manager = std::make_shared(); easynav::TFInfo tf_info; - auto init_result = manager->initialize(node, "routes", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + auto init_result = manager->initialize(node, "routes"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -169,7 +174,8 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenNoRoutesKey) auto manager = std::make_shared(); easynav::TFInfo tf_info; - auto init_result = manager->initialize(node, "routes", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + auto init_result = manager->initialize(node, "routes"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); const auto & routes = manager->get_routes(); @@ -201,7 +207,9 @@ TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) auto manager = std::make_shared(); easynav::TFInfo tf_info; - auto init_result = manager->initialize(node, "routes", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto init_result = manager->initialize(node, "routes"); ASSERT_TRUE(init_result.has_value()) << init_result.error(); easynav::NavState nav_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 3dab0cb6..4410d401 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 @@ -86,15 +86,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_info().map_frame; + 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_); @@ -102,7 +104,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) { @@ -117,7 +119,7 @@ SimpleMapsManager::on_initialize() }); static_map_.to_occupancy_grid(static_grid_msg_); - static_grid_msg_.header.frame_id = get_tf_info().map_frame; + static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); @@ -151,10 +153,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_info().map_frame) + .fuse(tf_info.map_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); @@ -169,7 +172,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_info().map_frame; + 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 e5598a27..e5437250 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 @@ -70,7 +70,9 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - manager->initialize(node, "test", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + manager->initialize(node, "test"); auto tf_buffer = easynav::RTTFBuffer::getInstance(node->get_clock()); tf2_ros::TransformListener tf_listener(*tf_buffer, *node, true); @@ -132,7 +134,9 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - manager->initialize(node, "test2", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + manager->initialize(node, "test2"); easynav::NavState navstate; @@ -179,7 +183,9 @@ TEST_F(SimpleMapsManagerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - manager->initialize(node, "test_savemap", tf_info); + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + manager->initialize(node, "test_savemap"); easynav::SimpleMap map_static; map_static.initialize(4, 4, 0.5, -1.0, -1.0, 0); 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 d58ad0e1..1998f9d4 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -26,6 +26,7 @@ #include #include "easynav_costmap_planner/CostmapPlanner.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -156,8 +157,9 @@ void CostmapPlanner::update(NavState & nav_state) const auto & map = nav_state.get("map.dynamic"); 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_info().map_frame) { + 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; 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 a0cde931..bc02c404 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -27,6 +27,7 @@ #include #include +#include "easynav_common/RTTFBuffer.hpp" #include "easynav_navmap_planner/AStarPlanner.hpp" #include "nav_msgs/msg/goals.hpp" @@ -95,8 +96,9 @@ void AStarPlanner::update(NavState & nav_state) 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_info().map_frame) { + 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()); 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 67c45535..899e29c6 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -22,6 +22,7 @@ #include #include "easynav_simple_planner/SimplePlanner.hpp" +#include "easynav_common/RTTFBuffer.hpp" #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -132,10 +133,11 @@ SimplePlanner::update(NavState & nav_state) 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 != get_tf_info().map_frame) { + 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; From ffe6672a6ca128b175646af911f921f25a75f0b9 Mon Sep 17 00:00:00 2001 From: migueldm Date: Thu, 11 Dec 2025 10:19:02 +0100 Subject: [PATCH 119/136] Fixed merge bug --- .../src/easynav_fusion_localizer/ukf_wrapper.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 index 1788ad08..659b05c1 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -894,7 +894,8 @@ void UkfWrapper::loadParams() map_frame_id_ = tf_info.map_frame; odom_frame_id_ = tf_info.odom_frame; base_link_frame_id_ = tf_info.robot_frame; - world_frame_id_ = tf_info.world_frame; + // World frame comes from Easynav TFInfo configuration + world_frame_id_ = tf_info_.map_frame; base_link_output_frame_id_ = base_link_frame_id_; @@ -924,8 +925,6 @@ void UkfWrapper::loadParams() * The default is the latter behavior (broadcast of odom->base_link). */ - // World frame comes from Easynav TFInfo configuration - world_frame_id_ = tf_info_.map_frame; if (map_frame_id_ == odom_frame_id_ || From eb61b59bd672c73bfb9d6ee0d5ae0fedcaf763c2 Mon Sep 17 00:00:00 2001 From: migueldm Date: Thu, 11 Dec 2025 10:55:31 +0100 Subject: [PATCH 120/136] More bugs fixed. Now compiles and works --- .../src/easynav_fusion_localizer/FusionLocalizer.cpp | 2 +- .../src/easynav_fusion_localizer/ukf_wrapper.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 0c5e93ed..f117357b 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -120,7 +120,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose // 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 = get_tf_info().map_frame; + pose_msg.header.frame_id = tf_info.map_frame; // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y) double utm_x, utm_y; 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 index 659b05c1..2ce767e9 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -895,7 +895,7 @@ void UkfWrapper::loadParams() 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; + world_frame_id_ = tf_info.map_frame; base_link_output_frame_id_ = base_link_frame_id_; From 1da206dd0b37e415e5a09fad5c1489ec3cb803dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 11 Dec 2025 20:25:18 +0100 Subject: [PATCH 121/136] Correct publication stamp to input stamp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_localizer/AMCLLocalizer.hpp | 3 +++ .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 12 ++++++++---- .../easynav_navmap_localizer/AMCLLocalizer.hpp | 3 +++ .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 13 ++++++------- .../easynav_simple_localizer/AMCLLocalizer.hpp | 3 +++ .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 9 +++++---- 6 files changed, 28 insertions(+), 15 deletions(-) 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 0c4d117d..d9d32e4f 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -222,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 882c40b7..68f29bfb 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -322,6 +322,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_; @@ -502,8 +503,11 @@ AMCLLocalizer::update_odom_from_tf() last_odom_ = odom_; odom_ = tf_odom; + last_input_time_ = tf_msg.header.stamp; initialized_odom_ = true; + + } void @@ -720,7 +724,7 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); + 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; @@ -736,7 +740,7 @@ 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.stamp = last_input_time_; array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { @@ -797,7 +801,7 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); + msg.header.stamp = last_input_time_; msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); @@ -820,7 +824,7 @@ AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); + 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; 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 6cbeab6b..954d9271 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -230,6 +230,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_; + /** * @brief Internal static map. */ 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 4fcebf88..be979e29 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -476,6 +476,7 @@ 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;} } @@ -493,6 +494,7 @@ void AMCLLocalizer::update_odom_from_tf() return; } tf2::fromMsg(tf_msg.transform, odom_); + last_input_time_ = tf_msg.header.stamp; initialized_odom_ = true; } @@ -569,12 +571,10 @@ void AMCLLocalizer::predict(NavState & nav_state) std::size_t sidx = 0; ::navmap::NavCelId cid = std::numeric_limits::max(); Eigen::Vector3f bary, hit_eig; - auto ta = get_node()->now(); const bool ok = navmap.locate_navcel( Eigen::Vector3f(static_cast(Pw.x()), static_cast(Pw.y()), static_cast(Pw.z())), sidx, cid, bary, &hit_eig, opts); - auto tb = get_node()->now(); if (ok) { tf2::Vector3 hit = to_tf(hit_eig); @@ -694,7 +694,6 @@ 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; @@ -959,7 +958,7 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) return; } geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); + 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; @@ -973,7 +972,7 @@ 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.stamp = last_input_time_; array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); for (const auto & p : particles_) { @@ -1069,7 +1068,7 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); + msg.header.stamp = last_input_time_; msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); @@ -1092,7 +1091,7 @@ nav_msgs::msg::Odometry AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); + 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; 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 671c11fc..2cb0c121 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -204,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 7cef1851..caa51253 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -311,6 +311,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_; @@ -526,7 +527,7 @@ void AMCLLocalizer::publishTF(const tf2::Transform & map2bf) { geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = get_node()->now(); + 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; @@ -542,7 +543,7 @@ 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.stamp = last_input_time_; array_msg.header.frame_id = tf_info.map_frame; array_msg.poses.reserve(particles_.size()); @@ -605,7 +606,7 @@ AMCLLocalizer::publishEstimatedPose(const tf2::Transform & est_pose) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); geometry_msgs::msg::PoseWithCovarianceStamped msg; - msg.header.stamp = get_node()->now(); + msg.header.stamp = last_input_time_; msg.header.frame_id = tf_info.map_frame; msg.pose.pose.position.x = mean.x(); @@ -628,7 +629,7 @@ AMCLLocalizer::get_pose() { nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = get_node()->now(); + 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; From 853b254c593f1a6128ebacfb5db50dbfdfd3cb6c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 15 Dec 2025 18:03:42 +0100 Subject: [PATCH 122/136] linting MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_fusion_localizer/ukf_wrapper.cpp | 1 - 1 file changed, 1 deletion(-) 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 index 2ce767e9..ec2f87da 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -926,7 +926,6 @@ void UkfWrapper::loadParams() */ - if (map_frame_id_ == odom_frame_id_ || odom_frame_id_ == base_link_frame_id_ || map_frame_id_ == base_link_frame_id_ || From 50855f20b93f4802e2294578f32c918bbe627556 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 16 Dec 2025 19:37:40 +0100 Subject: [PATCH 123/136] Remove C++20/C++23 features and update to new MethodBase interface --- .../easynav_costmap_common/costmap_2d.hpp | 1 + .../easynav_mpc_controller/CMakeLists.txt | 4 --- .../easynav_mpc_controller/MPCController.hpp | 4 +-- .../easynav_mpc_controller/MPCController.cpp | 4 +-- .../easynav_mppi_controller/CMakeLists.txt | 4 --- .../MPPIController.hpp | 5 ++-- .../MPPIController.cpp | 6 +---- .../easynav_serest_controller/CMakeLists.txt | 4 --- .../SerestController.hpp | 5 ++-- .../SerestController.cpp | 15 +++++------ .../easynav_simple_controller/CMakeLists.txt | 4 --- .../SimpleController.hpp | 5 ++-- .../SimpleController.cpp | 6 +---- .../easynav_vff_controller/CMakeLists.txt | 4 --- .../easynav_vff_controller/VffController.hpp | 8 ++---- .../easynav_vff_controller/VffController.cpp | 6 +---- .../easynav_costmap_localizer/CMakeLists.txt | 4 --- .../AMCLLocalizer.hpp | 4 +-- .../AMCLLocalizer.cpp | 5 +--- .../easynav_fusion_localizer/CMakeLists.txt | 4 --- .../FusionLocalizer.hpp | 9 ++++--- .../FusionLocalizer.cpp | 9 +++---- .../easynav_gps_localizer/CMakeLists.txt | 4 --- .../easynav_gps_localizer/GpsLocalizer.hpp | 6 ++--- .../easynav_gps_localizer/GpsLocalizer.cpp | 5 +--- .../easynav_navmap_localizer/CMakeLists.txt | 4 --- .../AMCLLocalizer.hpp | 4 +-- .../AMCLLocalizer.cpp | 4 +-- .../easynav_simple_localizer/CMakeLists.txt | 4 --- .../AMCLLocalizer.hpp | 4 +-- .../AMCLLocalizer.cpp | 5 +--- .../CMakeLists.txt | 4 --- .../BonxaiMapsManager.hpp | 4 +-- .../BonxaiMapsManager.cpp | 11 +++----- .../CMakeLists.txt | 4 --- .../CostmapMapsManager.hpp | 4 +-- .../filters/CostmapFilter.hpp | 5 ++-- .../filters/InflationFilter.hpp | 3 +-- .../filters/ObstacleFilter.hpp | 3 +-- .../CostmapMapsManager.cpp | 24 ++++++++---------- .../filters/CostmapFilter.cpp | 5 ++-- .../filters/InflationFilter.cpp | 5 +--- .../filters/ObstacleFilter.cpp | 7 ++---- .../CMakeLists.txt | 4 --- .../NavMapMapsManager.hpp | 4 +-- .../filters/InflationFilter.hpp | 2 +- .../filters/NavMapFilter.hpp | 5 ++-- .../filters/ObstacleFilter.hpp | 3 +-- .../NavMapMapsManager.cpp | 25 ++++++++----------- .../filters/InflationFilter.cpp | 5 +--- .../filters/NavMapFilter.cpp | 5 ++-- .../filters/ObstacleFilter.cpp | 7 ++---- .../CMakeLists.txt | 4 --- .../OctomapMapsManager.hpp | 4 +-- .../filters/InflationFilter.hpp | 4 +-- .../filters/ObstacleFilter.hpp | 3 +-- .../filters/OctomapFilter.hpp | 5 ++-- .../OctomapMapsManager.cpp | 15 +++++------ .../filters/InflationFilter.cpp | 5 +--- .../filters/ObstacleFilter.cpp | 7 ++---- .../filters/OctomapFilter.cpp | 5 ++-- .../CMakeLists.txt | 4 --- .../RoutesFilter.hpp | 6 ++--- .../RoutesMapsManager.hpp | 4 +-- .../filters/RoutesCostmapFilter.hpp | 6 ++--- .../RoutesMapsManager.cpp | 22 ++++++++-------- .../filters/RoutesCostmapFilter.cpp | 4 +-- .../tests/routes_costmap_filter_tests.cpp | 9 +++---- .../tests/routes_mapsmanager_tests.cpp | 15 ++++------- .../CMakeLists.txt | 4 --- .../SimpleMapsManager.hpp | 4 +-- .../SimpleMapsManager.cpp | 10 +++----- .../easynav_costmap_planner/CMakeLists.txt | 4 --- .../CostmapPlanner.hpp | 4 +-- .../CostmapPlanner.cpp | 3 +-- .../easynav_navmap_planner/CMakeLists.txt | 4 --- .../easynav_navmap_planner/AStarPlanner.hpp | 4 +-- .../easynav_navmap_planner/AStarPlanner.cpp | 4 +-- .../easynav_simple_planner/CMakeLists.txt | 4 --- .../easynav_simple_planner/SimplePlanner.hpp | 4 +-- .../easynav_simple_planner/SimplePlanner.cpp | 8 +++--- 81 files changed, 149 insertions(+), 329 deletions(-) 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 3779bec6..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 @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include "geometry_msgs/msg/point.hpp" diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index 48191bd0..ef47e161 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/controllers/easynav_mpc_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(easynav_common REQUIRED) find_package(easynav_core REQUIRED) diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index e01b6e6c..c8c31066 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -45,8 +45,8 @@ class MPCController : public ControllerMethodBase ~MPCController() override; /// \brief Initializes parameters and MPC 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_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index fde49925..939839c5 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -32,7 +32,7 @@ MPCController::MPCController() {} MPCController::~MPCController() = default; -std::expected +void MPCController::on_initialize() { auto node = get_node(); @@ -65,8 +65,6 @@ MPCController::on_initialize() detection_pub_ = node->create_publisher("/mpc/detection", 10); - - return {}; } void diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index 43a8650c..4edc78f1 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_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(easynav_common REQUIRED) find_package(easynav_core REQUIRED) 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 6f57ce38..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,7 +8,6 @@ #define EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_ #include -#include #include #include "geometry_msgs/msg/twist_stamped.hpp" @@ -33,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/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index 14d32002..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,8 +20,6 @@ /// \file /// \brief Implementation of the MPPIController class. -#include - #include "easynav_mppi_controller/MPPIController.hpp" #include "easynav_common/types/PointPerception.hpp" #include "easynav_common/RTTFBuffer.hpp" @@ -37,7 +35,7 @@ MPPIController::MPPIController() {} MPPIController::~MPPIController() = default; -std::expected +void MPPIController::on_initialize() { auto node = get_node(); @@ -72,8 +70,6 @@ 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( diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index 0067ae1f..110edf69 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/controllers/easynav_serest_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(easynav_common REQUIRED) find_package(easynav_core REQUIRED) 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 0cbe7a4c..2ca36808 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -20,7 +20,6 @@ #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_ -#include #include #include @@ -63,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). 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 25c282e6..48a7f464 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -41,7 +40,7 @@ namespace easynav SerestController::SerestController() = default; SerestController::~SerestController() = default; -std::expected +void SerestController::on_initialize() { auto node = get_node(); @@ -149,8 +148,6 @@ SerestController::on_initialize() last_vlin_ = 0.0; last_vrot_ = 0.0; last_update_ts_ = node->now(); - - return {}; } SerestController::PathData @@ -487,7 +484,7 @@ SerestController::should_turn_in_place( { // 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 * std::numbers::pi / 180.0; // enter TiP if |e_theta| > 60° + 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, @@ -510,7 +507,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 goal_yaw_tol = goal_yaw_tol_deg_ * std::numbers::pi / 180.0; + const double goal_yaw_tol = goal_yaw_tol_deg_ * M_PI / 180.0; if (dist_xy_goal > stop_r) { return false; @@ -625,7 +622,7 @@ SerestController::update_rt(NavState & nav_state) // 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_ * (std::numbers::pi / 180.0); + 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"); } @@ -727,7 +724,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 align_thr = 30.0 * std::numbers::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)); } @@ -778,7 +775,7 @@ SerestController::update_rt(NavState & nav_state) // Optional classic TiP safeguard using the same angular threshold { - const double turn_in_place_thr = (60.0 * std::numbers::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 d3542acb..4bd163a9 100644 --- a/controllers/easynav_simple_controller/CMakeLists.txt +++ b/controllers/easynav_simple_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(easynav_common REQUIRED) find_package(easynav_core REQUIRED) 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 2dfd5c7d..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,7 +8,6 @@ #define EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_ #include -#include #include #include "geometry_msgs/msg/point.hpp" @@ -32,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/src/easynav_simple_controller/SimpleController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp index 101bba50..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,8 +20,6 @@ /// \file /// \brief Implementation of the SimpleController class. -#include - #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -39,7 +37,7 @@ SimpleController::SimpleController() SimpleController::~SimpleController() = default; -std::expected +void SimpleController::on_initialize() { auto node = get_node(); @@ -87,8 +85,6 @@ SimpleController::on_initialize() last_vlin_ = 0.0; last_vrot_ = 0.0; last_update_ts_ = node->now(); - - return {}; } 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/include/easynav_vff_controller/VffController.hpp b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp index 980549b5..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,8 +23,6 @@ #ifndef EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ #define EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ -#include - #include "pcl/point_cloud.h" #include "easynav_core/ControllerMethodBase.hpp" @@ -83,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 9788ac4e..04729e3a 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -20,8 +20,6 @@ /// \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" @@ -35,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(); @@ -77,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 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/include/easynav_costmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp index d9d32e4f..44b8352b 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -67,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. 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 68f29bfb..5e2da2f7 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -20,7 +20,6 @@ /// \file /// \brief Implementation of the AMCLLocalizer class using Costmap2D. -#include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -187,7 +186,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +void AMCLLocalizer::on_initialize() { auto node = get_node(); @@ -272,8 +271,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) diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index 23a79bd0..4db3bed8 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_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_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index c96bb975..c29ce6e9 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -1,7 +1,6 @@ #pragma once #include -#include #include "easynav_core/LocalizerMethodBase.hpp" #include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado @@ -28,10 +27,12 @@ class FusionLocalizer : public easynav::LocalizerMethodBase /** * @brief Hook de inicialización de MethodBase. * - * Crea e inicializa el UkfWrapper, que cargará parámetros - * y creará todos los suscriptores/publicadores de robot_localization. + * Loads and initializes the UkfWrapper, which will load parameters + * and create all robot_localization subscribers/publishers. + * + * @throws std::runtime_error if initialization fails. */ - std::expected on_initialize() override; + void on_initialize() override; /** * @brief Hook de actualización RT (alta frecuencia) de LocalizerMethodBase. diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index f117357b..b212b73f 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -1,5 +1,3 @@ -#include - #include "easynav_fusion_localizer/FusionLocalizer.hpp" #include "easynav_localizer/LocalizerNode.hpp" @@ -16,7 +14,7 @@ namespace easynav { -std::expected FusionLocalizer::on_initialize() +void FusionLocalizer::on_initialize() { try { @@ -50,8 +48,8 @@ std::expected FusionLocalizer::on_initialize() RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", e.what()); - // Return an error to easynav - return std::unexpected(std::string("Failed to initialize UkfWrapper: ") + e.what()); + // Raise error + throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what()); } @@ -66,7 +64,6 @@ std::expected FusionLocalizer::on_initialize() n_gps_sensors_ = static_cast(ukf_wrapper_->getGpsCallbackDataArr().size()); RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); - return {}; } // 2. Hook de actualización RT (Tu "Timer" de alta frecuencia) 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/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp index fc731c9d..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,8 +23,6 @@ #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" @@ -61,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 5ed4393f..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,7 +27,7 @@ namespace easynav { -std::expected GpsLocalizer::on_initialize() +void GpsLocalizer::on_initialize() { auto node = get_node(); @@ -73,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) 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/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index 954d9271..23d2c45f 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -75,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. 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 be979e29..f6edaf8c 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 @@ -366,7 +365,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(); @@ -469,7 +468,6 @@ 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) 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/include/easynav_simple_localizer/AMCLLocalizer.hpp b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp index 2cb0c121..7bff4232 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -67,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. 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 caa51253..dffc2234 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -20,7 +20,6 @@ /// \file /// \brief Implementation of the AMCLLocalizer class. -#include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -185,7 +184,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +void AMCLLocalizer::on_initialize() { auto node = get_node(); @@ -263,8 +262,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) 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/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp index d9b2694f..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 @@ -61,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. 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 d2399860..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,7 +17,6 @@ // 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" @@ -53,7 +52,7 @@ BonxaiMapsManager::BonxaiMapsManager() BonxaiMapsManager::~BonxaiMapsManager() {} -std::expected +void BonxaiMapsManager::on_initialize() { auto node = get_node(); @@ -81,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; @@ -114,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); @@ -159,8 +158,6 @@ BonxaiMapsManager::on_initialize() Bonxai::WritePointsFromPCD(map_path_, bonxai_result); // This can overwrite yaml occ maps // ToDo }); - - return {}; } void 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/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp index 49687461..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 @@ -63,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. 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 f830830b..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,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ -#include #include #include "easynav_common/types/NavState.hpp" @@ -42,12 +41,12 @@ class CostmapFilter public: CostmapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(NavState & nav_state) = 0; const std::string & get_plugin_name() const; 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 23dd3476..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,7 +42,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ -#include #include #include @@ -79,7 +78,7 @@ class InflationFilter : public CostmapFilter public: InflationFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(NavState & nav_state); /** 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 7195ad0e..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,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ -#include #include #include "easynav_common/types/NavState.hpp" @@ -36,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 9b2b0379..e38c5a1a 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,7 +17,7 @@ // 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" @@ -52,7 +52,7 @@ CostmapMapsManager::CostmapMapsManager() CostmapMapsManager::~CostmapMapsManager() {} -std::expected +void CostmapMapsManager::on_initialize() { auto node = get_node(); @@ -81,13 +81,13 @@ CostmapMapsManager::on_initialize() std::shared_ptr instance; instance = costmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + costmap_filter); - - 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); @@ -97,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()); } } @@ -117,12 +117,12 @@ CostmapMapsManager::on_initialize() 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_); @@ -169,8 +169,6 @@ CostmapMapsManager::on_initialize() response->message = "Map successfully saved to: " + map_path_; } }); - - return {}; } void 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 7d2b8f2a..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,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" @@ -31,7 +30,7 @@ CostmapFilter::CostmapFilter() } -std::expected +void CostmapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name) @@ -39,7 +38,7 @@ CostmapFilter::initialize( parent_node_ = parent_node; plugin_name_ = plugin_name; - return on_initialize(); + on_initialize(); } std::shared_ptr 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 9e35b321..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,7 +39,6 @@ *********************************************************************/ -#include #include #include "easynav_costmap_common/costmap_2d.hpp" @@ -71,7 +70,7 @@ InflationFilter::InflationFilter() { } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); @@ -95,8 +94,6 @@ InflationFilter::on_initialize() cached_distances_.clear(); cached_costs_.clear(); need_reinflation_ = false; - - return {}; } void 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 b8a69cb5..234803cd 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,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_costmap_common/costmap_2d.hpp" @@ -39,11 +38,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(NavState & nav_state) diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index f1e05f04..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) 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 13f707a4..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 @@ -68,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 13bbc9f9..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 @@ -39,7 +39,7 @@ class InflationFilter : public NavMapFilter public: InflationFilter(); - virtual std::expected on_initialize() override; + virtual void on_initialize() override; virtual void update(::easynav::NavState & nav_state) override; bool inflate_layer_u8( 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 9bda7243..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,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ -#include #include #include "easynav_common/types/NavState.hpp" @@ -37,12 +36,12 @@ class NavMapFilter public: NavMapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, 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;} 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 bd07c70e..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,7 +21,6 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#include #include #include "navmap_core/NavMap.hpp" @@ -39,7 +38,7 @@ class ObstacleFilter : public NavMapFilter public: ObstacleFilter(); - virtual std::expected on_initialize() override; + virtual void on_initialize() override; virtual void update(::easynav::NavState & nav_state) override; virtual bool is_adding_layer() override {return true;} 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 c1cb22f8..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,7 +17,7 @@ // 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" @@ -63,7 +63,7 @@ NavMapMapsManager::NavMapMapsManager() NavMapMapsManager::~NavMapMapsManager() {} -std::expected +void NavMapMapsManager::on_initialize() { auto node = get_node(); @@ -93,13 +93,12 @@ NavMapMapsManager::on_initialize() std::shared_ptr instance; instance = navmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + navmap_filter); - - 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); @@ -109,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()); } } @@ -130,13 +129,13 @@ NavMapMapsManager::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; @@ -153,7 +152,7 @@ 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_)) { @@ -206,8 +205,6 @@ NavMapMapsManager::on_initialize() navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); // ToDo }); - - return {}; } void 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 2d9e8f7d..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,7 +39,6 @@ *********************************************************************/ -#include #include #include @@ -196,7 +195,7 @@ bool InflationFilter::inflate_layer_u8( return true; } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); @@ -217,8 +216,6 @@ InflationFilter::on_initialize() RCLCPP_INFO(node->get_logger(), "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f", inflation_radius_, cost_scaling_factor_, inscribed_radius_); - - return {}; } void InflationFilter::update(::easynav::NavState & nav_state) 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 970c8e17..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,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" @@ -33,7 +32,7 @@ NavMapFilter::NavMapFilter() } -std::expected +void NavMapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name) @@ -41,7 +40,7 @@ NavMapFilter::initialize( parent_node_ = parent_node; plugin_name_ = plugin_name; - return on_initialize(); + on_initialize(); } std::shared_ptr 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 c70dc614..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,7 +18,6 @@ // along with this program. If not, see . -#include #include #include @@ -42,11 +41,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(::easynav::NavState & nav_state) { 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/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp index ec358d5d..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 @@ -67,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 6561d7ec..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,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ -#include #include #include "octomap/octomap.h" @@ -38,12 +37,12 @@ class OctomapFilter public: OctomapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, 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_;} 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 347bd7e6..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,7 +17,6 @@ // 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" @@ -60,7 +59,7 @@ OctomapMapsManager::OctomapMapsManager() OctomapMapsManager::~OctomapMapsManager() {} -std::expected +void OctomapMapsManager::on_initialize() { auto node = get_node(); @@ -96,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()); // } // @@ -107,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()); // } // } @@ -121,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; @@ -144,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_)) { @@ -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 df331549..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) 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 ebc222e2..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,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "octomap/octomap.h" @@ -35,14 +34,14 @@ OctomapFilter::OctomapFilter() } -std::expected +void OctomapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - return on_initialize(); + on_initialize(); } std::shared_ptr diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt index f817f364..18cd1a91 100644 --- a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_routes_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_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp index f3b1e1ba..192af482 100644 --- 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 @@ -20,7 +20,6 @@ #ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ #define EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ -#include #include #include #include @@ -58,9 +57,8 @@ class RoutesFilter /// @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. - /// @return std::expected Empty on success or - /// an error message describing the failure. - virtual std::expected initialize( + /// @throws std::runtime_error if initialization fails. + virtual void initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns) = 0; 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 index c2f4705e..37fb17f3 100644 --- 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 @@ -94,9 +94,9 @@ class RoutesMapsManager : 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_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 index 449bb743..c910a916 100644 --- 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 @@ -20,7 +20,6 @@ #ifndef EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ #define EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ -#include #include #include "easynav_routes_maps_manager/RoutesFilter.hpp" @@ -54,9 +53,8 @@ class RoutesCostmapFilter : public RoutesFilter /// @param plugin_ns Namespace used to resolve this filter's /// parameters and topics. /// @param tf_info TF frame information used by the navigation stack. - /// @return std::expected Empty on success or an - /// error message on failure. - std::expected initialize( + /// @throws std::runtime_error if initialization fails. + void initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns) override; 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 index 12f27b29..2601842e 100644 --- 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 @@ -21,7 +21,6 @@ #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include #include #include @@ -54,7 +53,7 @@ RoutesMapsManager::RoutesMapsManager() RoutesMapsManager::~RoutesMapsManager() = default; -std::expected RoutesMapsManager::on_initialize() +void RoutesMapsManager::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -91,7 +90,7 @@ std::expected RoutesMapsManager::on_initialize() const auto pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + "/" + map_path_file; } else { - return std::unexpected( + throw std::runtime_error( "Parameters '" + plugin_name + ".package' and '" + plugin_name + ".map_path_file' are not correctly set"); } @@ -182,7 +181,7 @@ std::expected RoutesMapsManager::on_initialize() publish_routes_markers(); publish_interactive_markers(); } catch (const std::exception & e) { - return std::unexpected(std::string{"Failed to load routes: "} + e.what()); + throw std::runtime_error(std::string{"Failed to load routes: "} + e.what()); } // Instantiate and initialize configured route filters @@ -206,13 +205,14 @@ std::expected RoutesMapsManager::on_initialize() std::shared_ptr instance = routes_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + filter_name); - if (!result) { + 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(), result.error().c_str()); - return std::unexpected("Unable to initialize RoutesFilter " + plugin + - " . Error: " + result.error()); + 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); @@ -222,12 +222,10 @@ std::expected RoutesMapsManager::on_initialize() } catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR(node->get_logger(), "Unable to load plugin easynav::RoutesFilter. Error: %s", ex.what()); - return std::unexpected("Unable to load plugin easynav::RoutesFilter " + + throw std::runtime_error("Unable to load plugin easynav::RoutesFilter " + filter_name + " . Error: " + ex.what()); } } - - return {}; } void RoutesMapsManager::update(NavState & nav_state) 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 index adf222e6..83908e19 100644 --- 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 @@ -37,7 +37,7 @@ namespace easynav RoutesCostmapFilter::RoutesCostmapFilter() = default; -std::expected +void RoutesCostmapFilter::initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns) @@ -68,8 +68,6 @@ RoutesCostmapFilter::initialize( routes_occ_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes_map", rclcpp::QoS(1).reliable()); - - return {}; } void 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 index a0e2d988..40d8c72b 100644 --- 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 @@ -59,8 +59,7 @@ TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = filter.initialize(node, "routes.routes_costmap"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); NavState nav_state; // No 'routes' and no 'map.dynamic.filtered' keys -> update should not crash @@ -75,8 +74,7 @@ TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = filter.initialize(node, "routes.routes_costmap"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + 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); @@ -126,8 +124,7 @@ TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = filter.initialize(node, "routes.routes_costmap"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + 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); 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 index 6f845450..665a25a7 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -92,8 +92,7 @@ TEST_F(RoutesMapsManagerTest, LoadsRoutesFromValidYaml) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 2u); @@ -120,8 +119,7 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) auto manager = std::make_shared(); easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 1u); @@ -147,8 +145,7 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenYamlMissing) auto manager = std::make_shared(); easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 1u); @@ -175,8 +172,7 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenNoRoutesKey) auto manager = std::make_shared(); easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 1u); @@ -209,8 +205,7 @@ TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); easynav::NavState nav_state; manager->update(nav_state); 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/include/easynav_simple_maps_manager/SimpleMapsManager.hpp b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp index a7455b63..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 @@ -58,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 4410d401..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,8 +20,6 @@ /// \file /// \brief Implementation of the SimpleMapsManager class. -#include - #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "easynav_common/types/PointPerception.hpp" @@ -51,7 +49,7 @@ SimpleMapsManager::~SimpleMapsManager() } -std::expected +void SimpleMapsManager::on_initialize() { auto node = get_node(); @@ -71,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"); } } @@ -123,8 +121,6 @@ SimpleMapsManager::on_initialize() static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); - - return {}; } void 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/include/easynav_costmap_planner/CostmapPlanner.hpp b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp index c852a09c..0b13806b 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -54,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. 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 1998f9d4..59d6d762 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -123,7 +123,7 @@ CostmapPlanner::CostmapPlanner() }); } -std::expected CostmapPlanner::on_initialize() +void CostmapPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -139,7 +139,6 @@ std::expected CostmapPlanner::on_initialize() path_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; } void CostmapPlanner::update(NavState & nav_state) 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/include/easynav_navmap_planner/AStarPlanner.hpp b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp index c5fcb482..ce01f50c 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -58,9 +58,9 @@ class AStarPlanner : public PlannerMethodBase * Loads planner parameters, sets up ROS publishers, * 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. 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 bc02c404..044ddee9 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -63,7 +62,7 @@ AStarPlanner::AStarPlanner() }); } -std::expected AStarPlanner::on_initialize() +void AStarPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -76,7 +75,6 @@ std::expected AStarPlanner::on_initialize() path_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; } void AStarPlanner::update(NavState & nav_state) 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/include/easynav_simple_planner/SimplePlanner.hpp b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp index f54b9124..48bda79e 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -51,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 899e29c6..3315d178 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -86,7 +86,7 @@ SimplePlanner::SimplePlanner() }); } -std::expected +void SimplePlanner::on_initialize() { auto node = get_node(); @@ -99,8 +99,6 @@ SimplePlanner::on_initialize() path_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - - return {}; } void @@ -247,7 +245,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}); @@ -258,7 +256,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); From a07d155fa13d2faf1d8fbb611beb365bf2ff6527 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 19 Dec 2025 10:39:54 +0100 Subject: [PATCH 124/136] Collision checker scope was changed --- .../src/easynav_mpc_controller/MPCController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index a984b68c..73f28549 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -283,7 +283,7 @@ MPCController::update_rt(NavState & nav_state) std::cerr << "Optimization Error: " << e.what() << std::endl; } - if (collision_checker_active_) { + if (ControllerMethodBase::collision_checker_active_) { collision_checker(¶ms, u); } From 735468ef2c089b75fb52686beae6fb35e556b202 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 19 Dec 2025 12:32:56 +0100 Subject: [PATCH 125/136] Add a base_footprint frame in TFInfo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_serest_controller/SerestController.cpp | 4 ++-- .../tests/simple_controller_tests.cpp | 1 + .../src/easynav_vff_controller/VffController.cpp | 6 +++--- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 6 +++--- .../src/easynav_fusion_localizer/ukf_wrapper.cpp | 2 +- .../src/easynav_gps_localizer/GpsLocalizer.cpp | 4 ++-- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 7 ++++--- .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 4 ++-- .../tests/simple_localizer_tests.cpp | 1 + .../tests/simple_mapsmanager_tests.cpp | 3 +++ 10 files changed, 22 insertions(+), 16 deletions(-) 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 48a7f464..a0826748 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -306,7 +306,7 @@ SerestController::closest_obstacle_distance( auto fused = PointPerceptionsOpsView(perceptions) .downsample(0.3) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_frame) .filter({-dist_search_radius_, -dist_search_radius_, NAN}, {dist_search_radius_, dist_search_radius_, 2.0}) .collapse({NAN, NAN, 0.1}) @@ -370,7 +370,7 @@ SerestController::fetch_required_inputs( 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, tf_info.robot_frame); + publish_stop(nav_state, tf_info.robot_footprint_frame); return false; } diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index e783cd6c..eff6b1ea 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -148,6 +148,7 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test_savemap"); 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 04729e3a..1030276c 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -64,7 +64,7 @@ void VffController::on_initialize() // Initialize the odometry message cmd_vel_.header.stamp = node->now(); - cmd_vel_.header.frame_id = tf_info.robot_frame; + cmd_vel_.header.frame_id = tf_info.robot_footprint_frame; cmd_vel_.twist.linear.x = 0.0; cmd_vel_.twist.linear.y = 0.0; cmd_vel_.twist.linear.z = 0.0; @@ -262,14 +262,14 @@ void VffController::update_rt(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0}) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_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(); // Get VFF vectors - const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_frame); + const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_footprint_frame); // Use result vector to calculate output speed const auto & v = vff.result; 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 5e2da2f7..022892c4 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -488,7 +488,7 @@ AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf_info.odom_frame, tf_info.robot_footprint_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()); @@ -585,7 +585,7 @@ AMCLLocalizer::correct(NavState & nav_state) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.getResolution()) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.getResolution()) @@ -824,7 +824,7 @@ AMCLLocalizer::get_pose() 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; + odom_msg.child_frame_id = tf_info.robot_footprint_frame; tf2::Transform est_pose = getEstimatedPose(); 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 index ec2f87da..8bf14a01 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -893,7 +893,7 @@ void UkfWrapper::loadParams() map_frame_id_ = tf_info.map_frame; odom_frame_id_ = tf_info.odom_frame; - base_link_frame_id_ = tf_info.robot_frame; + base_link_frame_id_ = tf_info.robot_footprint_frame; // World frame comes from Easynav TFInfo configuration world_frame_id_ = tf_info.map_frame; 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 3a852fd9..694ceebf 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -35,7 +35,7 @@ void GpsLocalizer::on_initialize() odom_.header.stamp = get_node()->now(); 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_.child_frame_id = tf_info.robot_footprint_frame; // Create subscriber to GPS data gps_subscriber_ = node->create_subscription( @@ -114,7 +114,7 @@ void GpsLocalizer::update(NavState & nav_state) odom_.header.stamp = gps_msg_.header.stamp; 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_.child_frame_id = tf_info.robot_footprint_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/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index f6edaf8c..e84fb9b4 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -485,7 +485,7 @@ void AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - tf_info.odom_frame, tf_info.robot_frame, tf2::TimePointZero, + tf_info.odom_frame, tf_info.robot_footprint_frame, tf2::TimePointZero, tf2::durationFromSec(0.0)); } catch (const tf2::TransformException & ex) { RCLCPP_WARN(get_node()->get_logger(), "TF failed: %s", ex.what()); @@ -747,7 +747,8 @@ void AMCLLocalizer::correct(NavState & nav_state) 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(tf_info.robot_frame, b.frame_id); + T_bf_sensor_cache[b.frame_id] = lookup_bf_to_sensor(tf_info.robot_footprint_frame, + b.frame_id); // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; @@ -1092,7 +1093,7 @@ AMCLLocalizer::get_pose() 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; + odom_msg.child_frame_id = tf_info.robot_footprint_frame; pose_ = getEstimatedPose(); tf2::Transform est_pose = pose_; 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 dffc2234..791eed6e 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -387,7 +387,7 @@ void AMCLLocalizer::correct(NavState & nav_state) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.resolution()) - .fuse(tf_info.robot_frame) + .fuse(tf_info.robot_footprint_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.resolution()) @@ -629,7 +629,7 @@ AMCLLocalizer::get_pose() 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; + odom_msg.child_frame_id = tf_info.robot_footprint_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 0d48e663..fbb1ca18 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -108,6 +108,7 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test2"); 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 e5437250..87d5e3dd 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 @@ -70,6 +70,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test"); @@ -134,6 +135,7 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test2"); @@ -183,6 +185,7 @@ TEST_F(SimpleMapsManagerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; + tf_info.robot_footprint_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test_savemap"); From 21bf424fc90f535919dffddd1ae6fe87f75f644e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 1 Jan 2026 09:07:32 +0100 Subject: [PATCH 126/136] Adjust output time in costamp stack MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../SerestController.hpp | 6 ++- .../SerestController.cpp | 42 ++++++++++++------- .../AMCLLocalizer.cpp | 17 ++++---- .../CostmapMapsManager.cpp | 8 +++- .../filters/ObstacleFilter.cpp | 14 ++++--- .../CostmapPlanner.cpp | 11 +++++ 6 files changed, 67 insertions(+), 31 deletions(-) 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 2ca36808..0b908341 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -182,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. @@ -226,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( @@ -354,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/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index a0826748..2cf55119 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -287,7 +287,7 @@ SerestController::frenet_errors( double SerestController::closest_obstacle_distance( - const NavState & nav_state) const + const NavState & nav_state) { // 1) Prefer direct measurement if it exists if (nav_state.has("closest_obstacle_distance")) { @@ -304,14 +304,18 @@ SerestController::closest_obstacle_distance( const auto & perceptions = nav_state.get("points"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(0.3) - .fuse(tf_info.robot_footprint_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) - .as_points(); + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(0.3) + .fuse(tf_info.robot_footprint_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(); + + if (last_input_ts_ < view.get_latest_stamp()) { + last_input_ts_ = view.get_latest_stamp(); + } double min_dist = std::numeric_limits::infinity(); for (const auto p : fused) { @@ -377,6 +381,13 @@ SerestController::fetch_required_inputs( 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; @@ -427,7 +438,7 @@ 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); @@ -535,7 +546,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; @@ -578,7 +589,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_); @@ -601,7 +612,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; @@ -655,13 +666,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) 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 022892c4..0d2acb61 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -583,19 +583,22 @@ 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.getResolution()) - .fuse(tf_info.robot_footprint_frame) - .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .collapse({NAN, NAN, 0.1}) - .downsample(map_static.getResolution()) - .as_points(); + + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(map_static.getResolution()) + .fuse(tf_info.robot_footprint_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; } + last_input_time_ = view.get_latest_stamp(); + for (auto & particle : particles_) { int hits = 0; int possible_hits = 0; 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 e38c5a1a..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 @@ -195,6 +195,10 @@ CostmapMapsManager::update(NavState & nav_state) 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); } @@ -203,9 +207,11 @@ CostmapMapsManager::update(NavState & nav_state) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + rclcpp::Time map_stamp = nav_state.get("map_time"); + dynamic_map_->toOccupancyGridMsg(dynamic_grid_msg_); dynamic_grid_msg_.header.frame_id = tf_info.map_frame; - dynamic_grid_msg_.header.stamp = get_node()->now(); + 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/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index 234803cd..2021f63b 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 @@ -55,11 +55,15 @@ ObstacleFilter::update(NavState & nav_state) Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(dynamic_map.getResolution()) - .fuse(tf_info.map_frame) - .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .as_points(); + auto view = PointPerceptionsOpsView(perceptions); + view.downsample(dynamic_map.getResolution()) + .fuse(tf_info.map_frame) + .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); + + const auto stamp = view.get_latest_stamp(); + 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(); 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 59d6d762..4f1bc3d7 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -158,6 +158,17 @@ void CostmapPlanner::update(NavState & nav_state) 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 != tf_info.map_frame) { RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s", goals.header.frame_id.c_str()); From b8b8654c5df2af46460376901e816fd609471282 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 3 Jan 2026 08:36:59 +0100 Subject: [PATCH 127/136] Initialize last_input_time_ at costmap AMCL MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 1 + 1 file changed, 1 insertion(+) 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 0d2acb61..203bdbaa 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -269,6 +269,7 @@ AMCLLocalizer::on_initialize() "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); } From a6736cbf2a654352082cb0b541eb405e917da8c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 3 Jan 2026 10:09:40 +0100 Subject: [PATCH 128/136] Use last available TF for obstacle filter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2021f63b..40982c5d 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 @@ -57,7 +57,7 @@ ObstacleFilter::update(NavState & nav_state) auto view = PointPerceptionsOpsView(perceptions); view.downsample(dynamic_map.getResolution()) - .fuse(tf_info.map_frame) + .fuse(tf_info.map_frame, false) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); const auto stamp = view.get_latest_stamp(); From 0ab0ed4ff3c6b236d160bc3c80f30ce2b2e11f1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 3 Jan 2026 10:09:49 +0100 Subject: [PATCH 129/136] Improve navstate print including the time MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 3 ++- .../src/easynav_navmap_localizer/AMCLLocalizer.cpp | 3 ++- .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 3 ++- .../tests/costmap_mapsmanager_tests.cpp | 11 ----------- .../tests/navmap_mapsmanager_tests.cpp | 11 ----------- .../tests/simple_mapsmanager_tests.cpp | 12 ------------ .../src/easynav_costmap_planner/CostmapPlanner.cpp | 3 ++- .../src/easynav_navmap_planner/AStarPlanner.cpp | 3 ++- .../src/easynav_simple_planner/SimplePlanner.cpp | 3 ++- 9 files changed, 12 insertions(+), 40 deletions(-) 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 203bdbaa..75f51b21 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -177,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(); }); } 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 e84fb9b4..6bd2391c 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -357,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(); }); 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 791eed6e..30a62e70 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -175,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(); }); } 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 cb338cf0..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 @@ -19,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/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index e1ede33f..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 @@ -22,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_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 87d5e3dd..f46ee0fe 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 @@ -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 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 4f1bc3d7..2f9b356c 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -117,7 +117,8 @@ 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(); }); 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 044ddee9..0f1e7118 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -56,7 +56,8 @@ 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(); }); 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 3315d178..7d116d20 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -79,7 +79,8 @@ 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(); From 39b29365ccf77d39636810fa56ddc7587171c2ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 7 Jan 2026 11:58:39 +0100 Subject: [PATCH 130/136] Get timestamp from perception for stamping MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_maps_manager/filters/ObstacleFilter.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 40982c5d..1dd019e9 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 @@ -55,12 +55,13 @@ ObstacleFilter::update(NavState & nav_state) 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.map_frame, false) + .fuse(tf_info.map_frame, stamp, false) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); - const auto stamp = view.get_latest_stamp(); const auto & fused = view.as_points(); nav_state.set("map_time", stamp); From 4ee57adee0623d8d80217f0cc2de27f8f494844c Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Thu, 5 Feb 2026 13:08:40 +0100 Subject: [PATCH 131/136] Update CI to use container workers --- .github/workflows/humble.yaml | 11 +++-------- .github/workflows/humble_cron.yaml | 11 +++-------- .github/workflows/jazzy.yaml | 11 +++-------- .github/workflows/jazzy_cron.yaml | 11 +++-------- .github/workflows/kilted.yaml | 11 +++-------- .github/workflows/kilted_cron.yaml | 11 +++-------- .github/workflows/rolling.yaml | 10 +++------- .github/workflows/rolling_cron.yaml | 11 +++-------- 8 files changed, 24 insertions(+), 63 deletions(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index c98350fc..51cdc493 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -9,11 +9,9 @@ on: - humble jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-22.04] - fail-fast: false + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy steps: - name: Repo checkout uses: actions/checkout@v4 @@ -21,9 +19,6 @@ jobs: ref: humble - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: humble - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml index f195c9fa..82bb57ad 100644 --- a/.github/workflows/humble_cron.yaml +++ b/.github/workflows/humble_cron.yaml @@ -5,11 +5,9 @@ on: - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-22.04] - fail-fast: false + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy steps: - name: Repo checkout uses: actions/checkout@v4 @@ -17,9 +15,6 @@ jobs: ref: humble - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: humble - - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index bb185e0b..11301a4d 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,9 +21,6 @@ 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: diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index 38ebc4df..ac2a5efd 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.github/workflows/jazzy_cron.yaml @@ -5,11 +5,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 @@ -17,9 +15,6 @@ 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: diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 4e34d3b7..5eff233e 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -9,11 +9,9 @@ on: - kilted 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 @@ -21,9 +19,6 @@ 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: diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index 6167d6ce..dc93850f 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.github/workflows/kilted_cron.yaml @@ -5,11 +5,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 @@ -17,9 +15,6 @@ 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: diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index ea793087..7efd701a 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -9,11 +9,9 @@ on: - rolling 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@v6-beta @@ -21,8 +19,6 @@ jobs: 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: diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml index 39e4cd22..740df03b 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.github/workflows/rolling_cron.yaml @@ -5,11 +5,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@v6-beta @@ -17,9 +15,6 @@ jobs: 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: From 58efb28b99ecef452cb2e073d09a68c459aa2a33 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 15:23:34 +0100 Subject: [PATCH 132/136] Rolling changes were added --- .../MPPIController.cpp | 4 - .../SerestController.cpp | 33 ++++---- .../SimpleController.cpp | 4 - .../tests/simple_controller_tests.cpp | 2 +- .../easynav_vff_controller/VffController.cpp | 10 +-- .../AMCLLocalizer.cpp | 31 ++++---- .../easynav_fusion_localizer/ukf_wrapper.cpp | 2 +- .../easynav_gps_localizer/GpsLocalizer.cpp | 4 +- .../AMCLLocalizer.cpp | 6 +- .../AMCLLocalizer.cpp | 8 +- .../tests/simple_localizer_tests.cpp | 2 +- .../filters/ObstacleFilter.cpp | 10 +-- .../filters/ObstacleFilter.cpp | 7 -- .../SimpleMapsManager.cpp | 4 - .../tests/simple_mapsmanager_tests.cpp | 6 +- .../easynav_navmap_planner/AStarPlanner.hpp | 28 ------- .../easynav_navmap_planner/AStarPlanner.cpp | 76 ------------------- 17 files changed, 51 insertions(+), 186 deletions(-) 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 6c330088..b471b039 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -190,11 +190,7 @@ MPPIController::update_rt(NavState & nav_state) 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}) -<<<<<<< HEAD - .fuse("map") -======= .fuse(tf_info.map_frame) ->>>>>>> juanscelyg/rolling .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(0.1) 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 da2c3b02..df57b5ca 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -301,33 +301,34 @@ SerestController::closest_obstacle_distance( // 2) Analyze distance sensors if (!nav_state.has("points")) {return std::numeric_limits::infinity();} -<<<<<<< HEAD - 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_footprint_frame) + .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(); - if (last_input_ts_ < view.get_latest_stamp()) { - last_input_ts_ = view.get_latest_stamp(); + 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); } ->>>>>>> juanscelyg/rolling double min_dist = std::numeric_limits::infinity(); for (const auto p : fused) { @@ -386,7 +387,7 @@ SerestController::fetch_required_inputs( 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, tf_info.robot_footprint_frame); + publish_stop(nav_state, tf_info.robot_frame); return false; } 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 41042493..9438c50f 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -112,11 +112,7 @@ SimpleController::update_rt(NavState & nav_state) } // If we're very close to the final path pose, stop the robot. -<<<<<<< HEAD - const auto pose = nav_state.get("robot_pose").pose.pose; -======= const auto & pose = nav_state.get("robot_pose").pose.pose; ->>>>>>> juanscelyg/rolling 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 eff6b1ea..4af0d5c2 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -148,7 +148,7 @@ TEST_F(AMCLLocalizerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; + tf_info.robot_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test_savemap"); 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 bf17d635..594ae7c4 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -64,7 +64,7 @@ void VffController::on_initialize() // Initialize the odometry message cmd_vel_.header.stamp = node->now(); - cmd_vel_.header.frame_id = tf_info.robot_footprint_frame; + 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; @@ -262,18 +262,14 @@ void VffController::update_rt(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .filter({-10.0, -10.0, -10.0}, {10.0, 10.0, 10.0}) -<<<<<<< HEAD - .fuse(get_tf_prefix() + "base_link") -======= - .fuse(tf_info.robot_footprint_frame) ->>>>>>> juanscelyg/rolling + .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(); // Get VFF vectors - const VFFVectors & vff = get_vff(angle_error, fused, tf_info.robot_footprint_frame); + 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/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index c5ff1c59..e480ff6a 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -490,7 +490,7 @@ AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - tf_info.odom_frame, tf_info.robot_footprint_frame, tf2::TimePointZero, + 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()); @@ -584,32 +584,35 @@ AMCLLocalizer::correct(NavState & nav_state) const auto & map_static = nav_state.get("map.static"); -<<<<<<< HEAD - 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_footprint_frame) + .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(); ->>>>>>> juanscelyg/rolling if (filtered.empty()) { RCLCPP_WARN(get_node()->get_logger(), "No points to correct"); return; } - last_input_time_ = view.get_latest_stamp(); + 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; @@ -839,7 +842,7 @@ AMCLLocalizer::get_pose() 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_footprint_frame; + odom_msg.child_frame_id = tf_info.robot_frame; tf2::Transform est_pose = getEstimatedPose(); 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 index 8bf14a01..ec2f87da 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -893,7 +893,7 @@ void UkfWrapper::loadParams() map_frame_id_ = tf_info.map_frame; odom_frame_id_ = tf_info.odom_frame; - base_link_frame_id_ = tf_info.robot_footprint_frame; + base_link_frame_id_ = tf_info.robot_frame; // World frame comes from Easynav TFInfo configuration world_frame_id_ = tf_info.map_frame; 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 694ceebf..3a852fd9 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -35,7 +35,7 @@ void GpsLocalizer::on_initialize() odom_.header.stamp = get_node()->now(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); odom_.header.frame_id = tf_info.map_frame; - odom_.child_frame_id = tf_info.robot_footprint_frame; + odom_.child_frame_id = tf_info.robot_frame; // Create subscriber to GPS data gps_subscriber_ = node->create_subscription( @@ -114,7 +114,7 @@ void GpsLocalizer::update(NavState & nav_state) odom_.header.stamp = gps_msg_.header.stamp; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); odom_.header.frame_id = tf_info.map_frame; - odom_.child_frame_id = tf_info.robot_footprint_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/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index 6bd2391c..c7615292 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -486,7 +486,7 @@ void AMCLLocalizer::update_odom_from_tf() geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( - tf_info.odom_frame, tf_info.robot_footprint_frame, tf2::TimePointZero, + 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()); @@ -748,7 +748,7 @@ void AMCLLocalizer::correct(NavState & nav_state) 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(tf_info.robot_footprint_frame, + 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]; @@ -1094,7 +1094,7 @@ AMCLLocalizer::get_pose() 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_footprint_frame; + odom_msg.child_frame_id = tf_info.robot_frame; pose_ = getEstimatedPose(); tf2::Transform est_pose = pose_; 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 e81100bd..693d006e 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -388,11 +388,7 @@ void AMCLLocalizer::correct(NavState & nav_state) const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) .downsample(map_static.resolution()) -<<<<<<< HEAD - .fuse(get_tf_prefix() + "base_footprint") -======= - .fuse(tf_info.robot_footprint_frame) ->>>>>>> juanscelyg/rolling + .fuse(tf_info.robot_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .collapse({NAN, NAN, 0.1}) .downsample(map_static.resolution()) @@ -634,7 +630,7 @@ AMCLLocalizer::get_pose() 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_footprint_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 fbb1ca18..731a256e 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -108,7 +108,7 @@ TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; + tf_info.robot_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test2"); 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 07716536..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 @@ -55,18 +55,11 @@ ObstacleFilter::update(NavState & nav_state) Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); -<<<<<<< HEAD - auto fused = PointPerceptionsOpsView(perceptions) - .downsample(dynamic_map.getResolution()) - .fuse(get_tf_prefix() + "map") - .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) - .as_points(); -======= rclcpp::Time stamp; auto view = PointPerceptionsOpsView(perceptions); view.downsample(dynamic_map.getResolution()) - .fuse(tf_info.map_frame, stamp, false) + .fuse(tf_info.robot_frame) .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}); const auto & fused = view.as_points(); @@ -77,7 +70,6 @@ ObstacleFilter::update(NavState & nav_state) double min_y = std::numeric_limits::max(); double max_x = std::numeric_limits::lowest(); double max_y = std::numeric_limits::lowest(); ->>>>>>> juanscelyg/rolling for (const auto & p : fused) { min_x = std::min(min_x, static_cast(p.x)); 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 ae25ebf8..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 @@ -56,17 +56,10 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) navmap_.layer_clear(get_layer_name(), navmap_ros::FREE_SPACE); -<<<<<<< HEAD - 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) ->>>>>>> juanscelyg/rolling .as_points(); const float voxel_xy = 0.30f; 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 637348c5..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 @@ -153,11 +153,7 @@ SimpleMapsManager::update(NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(dynamic_map_.resolution()) -<<<<<<< HEAD - .fuse(get_tf_prefix() + "map") -======= .fuse(tf_info.map_frame) ->>>>>>> juanscelyg/rolling .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); 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 82435ed1..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 @@ -58,7 +58,7 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; + tf_info.robot_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test"); @@ -123,7 +123,7 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; + tf_info.robot_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test2"); @@ -173,7 +173,7 @@ TEST_F(SimpleMapsManagerTest, SavemapServiceWorks) tf_info.map_frame = "world_map"; tf_info.odom_frame = "world_odom"; tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; + tf_info.robot_frame = "world_footprint_base"; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); manager->initialize(node, "test_savemap"); 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 3b9722e7..ce01f50c 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -104,34 +104,6 @@ class AStarPlanner : public PlannerMethodBase */ void ensure_graph_cache(const ::navmap::NavMap & map); - /** - * @brief Smooth a Path in XY while keeping every waypoint inside its original NavCel. - * - * The algorithm performs several iterations of Laplacian smoothing on XY: - * p_i' = (1 - alpha) * p_i + alpha * 0.5 * (p_{i-1} + p_{i+1}) - * For each i, the candidate point is clamped to the original triangle (NavCel) - * using closest-point-on-triangle, so it never leaves that NavCel. The final z' - * is the triangle height at the resulting (x', y'). - * - * Endpoints are kept fixed. Optionally, points forming a sharp angle are also - * kept (see `corner_keep_deg`). - * - * @param in_path Input nav_msgs::msg::Path (world coordinates). - * @param navmap The NavMap where the path lies on. - * @param iterations Number of smoothing iterations (>= 1). Default: 5. - * @param alpha Smoothing factor in (0, 0.5]. Default: 0.4. - * @param corner_keep_deg Angle threshold (degrees): if the interior angle at a point - * is below this value, the point is kept as an anchor. Set <= 0 - * to disable. Default: 0 (disabled). - * @return nav_msgs::msg::Path Smoothed path, same frame_id and header stamp as input. - */ - nav_msgs::msg::Path path_smoother( - const nav_msgs::msg::Path & in_path, - const ::navmap::NavMap & navmap, - int iterations = 5, - float alpha = 0.4f, - float corner_keep_deg = 0.0f); - /** * @brief Smooth a Path in XY while keeping every waypoint inside its original NavCel. * 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 4d46ebca..0f1e7118 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -85,23 +85,13 @@ void AStarPlanner::update(NavState & nav_state) return; } -<<<<<<< HEAD - - 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")) { ->>>>>>> juanscelyg/rolling nav_state.set("path", current_path_); return; } -<<<<<<< HEAD - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); -======= const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); ->>>>>>> juanscelyg/rolling const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; @@ -123,11 +113,7 @@ void AStarPlanner::update(NavState & nav_state) } current_goal_ = goal; -<<<<<<< HEAD - auto poses = a_star_path(navmap_, robot_pose.pose.pose, goal); -======= auto poses = a_star_path(navmap, robot_pose.pose.pose, goal); ->>>>>>> juanscelyg/rolling if (!poses.empty()) { current_path_.header.stamp = get_node()->now(); current_path_.header.frame_id = goals.header.frame_id; @@ -140,11 +126,7 @@ void AStarPlanner::update(NavState & nav_state) current_path_.poses.push_back(std::move(ps)); } -<<<<<<< HEAD - current_path_ = path_smoother(current_path_, navmap_); -======= current_path_ = path_smoother(current_path_, navmap); ->>>>>>> juanscelyg/rolling if (path_pub_->get_subscription_count() > 0) { path_pub_->publish(current_path_); @@ -153,10 +135,6 @@ void AStarPlanner::update(NavState & nav_state) nav_state.set("path", current_path_); } -<<<<<<< HEAD - -======= ->>>>>>> juanscelyg/rolling nav_msgs::msg::Path AStarPlanner::path_smoother( const nav_msgs::msg::Path & in_path, @@ -181,16 +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; -<<<<<<< HEAD - 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)); ->>>>>>> juanscelyg/rolling } // Use walking hints to speed up sequential location @@ -226,12 +198,8 @@ AStarPlanner::path_smoother( } // Helper to fetch triangle vertices (A,B,C) for a cid -<<<<<<< HEAD - auto get_triangle_vertices = [&](::navmap::NavCelId cid) -> std::array { -======= auto get_triangle_vertices = [&](::navmap::NavCelId cid) -> std::array { ->>>>>>> juanscelyg/rolling 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]); @@ -240,13 +208,8 @@ AStarPlanner::path_smoother( }; // Helper: clamp a 3D point to the triangle of a given cid (closest point) -<<<<<<< HEAD - 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 { ->>>>>>> juanscelyg/rolling const auto V = get_triangle_vertices(cid); return ::navmap::closest_point_on_triangle(p, V[0], V[1], V[2]); }; @@ -301,11 +264,7 @@ AStarPlanner::path_smoother( // Clamp to the *original* triangle of this point Eigen::Vector3f clamped = clamp_to_triangle(cand3, cids[i]); -<<<<<<< HEAD - next[i] = clamped; // already lies on triangle plane, z' consistent -======= next[i] = clamped; // already lies on triangle plane, z' consistent ->>>>>>> juanscelyg/rolling } curr.swap(next); } @@ -321,8 +280,6 @@ AStarPlanner::path_smoother( return out; } -<<<<<<< HEAD -======= // 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) { @@ -365,7 +322,6 @@ void AStarPlanner::ensure_graph_cache(const ::navmap::NavMap & map) } } ->>>>>>> juanscelyg/rolling std::vector AStarPlanner::a_star_path( const ::navmap::NavMap & nm, const geometry_msgs::msg::Pose & start, @@ -398,25 +354,6 @@ std::vector AStarPlanner::a_star_path( if (!nm.closest_navcel(pG, sidx_g, cid_goal, q, d2)) {return {};} } -<<<<<<< HEAD - const 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()}; - } - - auto h = [&](NavCelId a, NavCelId b) -> double { - const auto d = C[a] - C[b]; - return static_cast(d.norm()); - }; - - auto step_cost = [&](NavCelId from, NavCelId to) -> double { - const double dist = static_cast((C[from] - C[to]).norm()); - return dist; -======= const std::size_t N = nm.navcels.size(); // 2) Choose cost layer: prefer "inflated_obstacles", fallback to "obstacles" @@ -442,7 +379,6 @@ std::vector AStarPlanner::a_star_path( auto traversable = [&](NavCelId c) -> bool { const std::uint8_t v = occ_[c]; return (v != LETHAL_OBSTACLE) && (v != NO_INFORMATION); ->>>>>>> juanscelyg/rolling }; // Normalize a uint8_t cost into [0, 1]; FREE_SPACE=0 → 0.0; INSCRIBED=253 → ~1.0. @@ -523,14 +459,8 @@ std::vector AStarPlanner::a_star_path( continue; } -<<<<<<< HEAD - 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;} ->>>>>>> juanscelyg/rolling const double sc = step_cost(u, v); if (!std::isfinite(sc)) {continue;} @@ -556,15 +486,9 @@ std::vector AStarPlanner::a_star_path( c = parent_[c]) { geometry_msgs::msg::Pose p; -<<<<<<< HEAD - 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(); ->>>>>>> juanscelyg/rolling p.orientation = goal.orientation; path.push_back(std::move(p)); if (c == cid_start) {break;} From 5e8eaaa9aa3a877bcda421ced1504e472fb7996f Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 15:44:46 +0100 Subject: [PATCH 133/136] CI Fixed --- .../easynav_mpc_controller/MPCController.cpp | 3 ++- .../SerestController.cpp | 20 +++++++++---------- .../AMCLLocalizer.cpp | 20 +++++++++---------- .../filters/ObstacleFilter.cpp | 4 ---- 4 files changed, 22 insertions(+), 25 deletions(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index d042407c..66ec8eea 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -119,7 +119,8 @@ MPCController::collision_checker(void *data, std::vector & u) 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) ); + "[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_; 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 df57b5ca..b0fb4e0e 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -314,17 +314,17 @@ SerestController::closest_obstacle_distance( 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; + 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; - }; + return latest_stamp; + }; if (last_input_ts_ < latest_time(perceptions)) { last_input_ts_ = latest_time(perceptions); 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 e480ff6a..48bc71ea 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -600,17 +600,17 @@ AMCLLocalizer::correct(NavState & nav_state) } 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; + 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; - }; + return latest_stamp; + }; last_input_time_ = latest_time(perceptions); 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 c7bbac92..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 @@ -64,11 +64,7 @@ ObstacleFilter::update(::easynav::NavState & nav_state) auto fused = PointPerceptionsOpsView(perceptions) .downsample(get_map_resolution()) -<<<<<<< HEAD - .fuse(get_tf_prefix() + "map") -======= .fuse(tf_info.map_frame) ->>>>>>> juanscelyg/rolling .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN}) .as_points(); From baa62e39392c8912496710d0220c0af67f94be1b Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 16:10:50 +0100 Subject: [PATCH 134/136] Documentation was corrected --- .github/workflows/rolling.yaml | 4 -- README.md | 3 -- controllers/easynav_mppi_controller/README.md | 4 -- .../easynav_serest_controller/README.md | 6 --- .../easynav_simple_controller/README.md | 54 ------------------- 5 files changed, 71 deletions(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index eefbf8ba..7efd701a 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -14,11 +14,7 @@ jobs: image: ubuntu:noble steps: - name: Repo checkout -<<<<<<< HEAD - uses: actions/checkout@v4 -======= uses: actions/checkout@v6-beta ->>>>>>> juanscelyg/rolling with: ref: rolling - name: Setup ROS 2 diff --git a/README.md b/README.md index f83bc966..583ff9a6 100644 --- a/README.md +++ b/README.md @@ -4,12 +4,9 @@ [![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) -<<<<<<< HEAD -======= [![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) ->>>>>>> juanscelyg/rolling ## Description diff --git a/controllers/easynav_mppi_controller/README.md b/controllers/easynav_mppi_controller/README.md index 18c2a626..b6d7f1e9 100644 --- a/controllers/easynav_mppi_controller/README.md +++ b/controllers/easynav_mppi_controller/README.md @@ -1,11 +1,7 @@ # easynav_mppi_controller -<<<<<<< HEAD [![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)](#) - -======= ->>>>>>> juanscelyg/rolling ## Description A Model Predictive Path Integral (MPPI) controller implementation for Easy Navigation. diff --git a/controllers/easynav_serest_controller/README.md b/controllers/easynav_serest_controller/README.md index f0424a48..8a36df77 100644 --- a/controllers/easynav_serest_controller/README.md +++ b/controllers/easynav_serest_controller/README.md @@ -1,11 +1,8 @@ # easynav_serest_controller -<<<<<<< HEAD [![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)](#) -======= ->>>>>>> juanscelyg/rolling ## Description A SeReST (Smooth Error-Responsive Speed and Turning) controller for path tracking. @@ -19,11 +16,8 @@ A SeReST (Smooth Error-Responsive Speed and Turning) controller for path trackin | Distribution | Status | |---|---:| -<<<<<<< HEAD -======= | humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | | jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | ->>>>>>> juanscelyg/rolling | 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) | diff --git a/controllers/easynav_simple_controller/README.md b/controllers/easynav_simple_controller/README.md index b5a9777c..e9ae782d 100644 --- a/controllers/easynav_simple_controller/README.md +++ b/controllers/easynav_simple_controller/README.md @@ -1,14 +1,8 @@ # easynav_simple_controller -<<<<<<< HEAD [![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 -======= - -## Description - ->>>>>>> juanscelyg/rolling 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 @@ -20,20 +14,13 @@ Simple path-following controller that uses PID controllers and a look-ahead refe | Distribution | Status | |---|---:| -<<<<<<< HEAD -======= | humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | | jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | ->>>>>>> juanscelyg/rolling | 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) -<<<<<<< HEAD -======= - ->>>>>>> juanscelyg/rolling - **Plugin Name:** `easynav_simple_controller/SimpleController` - **Type:** `easynav::SimpleController` - **Base Class:** `easynav::ControllerMethodBase` @@ -41,26 +28,6 @@ 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 -<<<<<<< HEAD -All parameters are declared under the plugin namespace, i.e., `//easynav_simple_controller/SimpleController/...`. - -| 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. -======= All parameters are declared under the plugin namespace, i.e., `//easynav_simple_controller/SimpleController/...`. @@ -83,30 +50,10 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `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. | ->>>>>>> juanscelyg/rolling ## Interfaces (NavState, Topics and Services) ### NavState -<<<<<<< HEAD -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. - -### 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. - - -## 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 -======= This controller uses the shared `NavState` bag provided by the `easynav_core` framework. The following keys are used at runtime by `SimpleController`: @@ -127,4 +74,3 @@ This controller reads pose from `nav_msgs/Odometry` (NavState key `robot_pose`). ## License GPL-3.0-only ->>>>>>> juanscelyg/rolling From 4e1976fe8372e8c108df24ec02679d4934c3004c Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Mon, 16 Feb 2026 17:58:57 +0100 Subject: [PATCH 135/136] Fixed CI --- .github/workflows/jazzy.yaml | 3 +-- .github/workflows/jazzy_cron.yaml | 4 +--- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 11301a4d..615e5cb4 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -24,9 +24,8 @@ jobs: - 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_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: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index ac2a5efd..dfd56507 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.github/workflows/jazzy_cron.yaml @@ -18,10 +18,8 @@ jobs: - 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_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: jazzy - ref: jazzy - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { From 5be04c1146351d00f9cd9ddfc81d7eb3f64139f3 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Wed, 18 Feb 2026 10:02:53 +0100 Subject: [PATCH 136/136] CI updated --- .github/workflows/jazzy.yaml | 2 +- .github/workflows/jazzy_cron.yaml | 2 +- .github/workflows/kilted.yaml | 3 +-- .github/workflows/kilted_cron.yaml | 4 +--- .github/workflows/rolling_cron.yaml | 2 +- 5 files changed, 5 insertions(+), 8 deletions(-) diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 615e5cb4..365cdccf 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -24,7 +24,7 @@ jobs: - 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_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 skip-tests: false colcon-defaults: | diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index dfd56507..c7049a98 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.github/workflows/jazzy_cron.yaml @@ -18,7 +18,7 @@ jobs: - 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_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 skip-tests: false colcon-defaults: | diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 5eff233e..0a4f68ed 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -22,9 +22,8 @@ jobs: - 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: false colcon-defaults: | { diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index dc93850f..c4b6b82b 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.github/workflows/kilted_cron.yaml @@ -18,10 +18,8 @@ jobs: - 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 - ref: kilted - vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false colcon-defaults: | { diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml index 740df03b..ef59286d 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.github/workflows/rolling_cron.yaml @@ -18,7 +18,7 @@ jobs: - 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 + 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